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+ Integer.toString(lamps.get(i).getColor()[1]) +" " 
+ Integer.toString(lamps.get(i).getColor()[2]); 

} 

saveStrings(lampsFile, lines); 


} 


The function loadLamps() performs the inverse task to the previous one. You use the Processing 
function loadStrings() to save each line in the file to a string in the array of strings stringArray, and 
then you split each line at the blank spaces and convert each substring into floats defining the position 
of the lamp and three integers defining its color. You then create the necessary lamps and set their color 
values to the ones loaded from the file. 
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void loadLamps() { 

String lines[] = loadStrings(lampsFile) ; 

for (int i = 0; i < lines.length; i++) { 
String[] coordinates = lines[i].split(" "); 
Lamp lampTemp = new Lamp(Float.valueOf(coordinates[0]), 
Float.valueOf(coordinates[1]), 
Float.valueOf(coordinates[2])); 
lampTemp.setColor(Integer.valueOf(coordinates[3]), 
Integer. valueOf(coordinates[4]), 
Integer. valueOf(coordinates[5])); 
lamps.add(lampTemp) ; 


These two functions are called when you press the s and | (the letter 1, not number 1) keys on your 
keyboard. The Processing callback function keyPressed() takes care of invoking them when a new 
keyboard event is triggered. 


public void keyPressed() { 
switch (key) { 
case ''S': 
saveLamps(); 
break; 
case 'L': 
loadLamps(); 
break; 
} 
} 


Serial Communication 


The serial function performs the task of sending the necessary data to coordinate the physical lamps to 
the ones you are seeing on your screen. For each lamp you send the event trigger ‘S’, followed by the 
number of the lamp the message is intended for, and then the W, R, G, and B values. 


void sendSerialData() { 
for (int i = 0; i < lamps.size(); i++) { 
myPort .write('S'); 
myPort .write(i) ; 
myPort .write(lamps.get(i).W); 
myPort .write(lamps.get(i).R); 
myPort .write(lamps.get(i).G); 
myPort .write(lamps.get(i).B); 


Display Functions 


The following functions are used to print the Kinect point clouds and user skeleton on screen. You saw 
how to display the Kinect point cloud in Chapter 3. 


void drawPointCloud(int steps) { 
int[] depthMap = kinect.depthMap(); 
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int index; 
PVector realWorldPoint; 
stroke(255); 
for (int y = 0; y < kinect.depthHeight(); y += steps) { 
for (int x = 0; x < kinect.depthWidth(); x += steps) { 
index = x + y * kinect.depthWidth(); 
if (depthMap[index] > 0) { 
realWorldPoint = kinect.depthMapRealWorld() [index]; 
point(realWorldPoint.x, realWorldPoint.y, realWorldPoint.z); 


The drawing of the user points builds on the same principles as the previous function, but you sort 
the depth map points according to the color of the equivalent point in the user map. The user map is an 
array of pixels with value 0 if the Kinect determines they are background pixels, and a higher value, 
starting at 1, for the pixels defining the users. 

This function allows you to print the user’s pixels in a different color, so you can observe on screen 
what Kinect is considering to be your user. 


void drawUserPoints(int steps) { 
int[] userMap = kinect.getUsersPixels(SimpleOpenNI.USERS ALL); 
// draw the 3d point depth map 
PVector[] realWorldPoint = new PVector[kinect.depthHeight() * kinect.depthWidth()]; 
int index; 
pushStyle(); 
stroke(255); 
for (int y = 0; y < kinect.depthHeight(); y += steps) { 
for (int x = 0; x < kinect.depthWidth(); x += steps) { 
index = x + y * kinect.depthWidth(); 
realWorldPoint[index] = kinect.depthMapRealWorld()[index].get(); 
if (userMap[index] != 0) { 
strokeWeight (2); 
stroke(0, 255, 0); 
point (realWorldPoint[index].x, realWorldPoint[index].y, 
realWorldPoint[ index].z); 
} 
} 


; ropstyle(); 


The next two functions are from Simple-OpenNI’s 3d skeleton tracking example. They both use 
functions specific to Simple-OpenNI to draw the user’s limbs on screen. 


void drawSkeleton(int userId) { 
strokeWeight (3); 
// to get the 3d joint data 
drawLimb(userId, SimpleOpenNI.SKEL_HEAD, SimpleOpenNI.SKEL_NECK); 
drawLimb(userId, SimpleOpenNI.SKEL_NECK, 
SimpleOpenNI.SKEL_LEFT SHOULDER) ; 
drawLimb(userId, SimpleOpenNI.SKEL_LEFT SHOULDER, 
SimpleOpenNI.SKEL_LEFT ELBOW) ; 
drawLimb(userId, SimpleOpenNI.SKEL_LEFT ELBOW, 


} 
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SimpleOpenNI.SKEL_LEFT_HAND) ; 


drawLimb(userId, SimpleOpenNI.SKEL_NECK, 
SimpleOpenNI.SKEL_RIGHT SHOULDER) ; 
drawLimb(userId, SimpleOpenNI.SKEL_RIGHT SHOULDER, 
SimpleOpenNI.SKEL_RIGHT ELBOW); 

drawLimb(userId, SimpleOpenNI.SKEL_RIGHT ELBOW, 
SimpleOpenNI.SKEL_RIGHT_ HAND) ; 


drawLimb(userId, SimpleOpenNI.SKEL_LEFT SHOULDER, 
SimpleOpenNI.SKEL_TORSO) ; 
drawLimb(userId, SimpleOpenNI.SKEL_RIGHT SHOULDER, 
SimpleOpenNI.SKEL_TORSO) ; 


drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_LEFT HIP); 
drawLimb(userId, SimpleOpenNI.SKEL_LEFT HIP, 
SimpleOpenNI.SKEL_LEFT_KNEE); 

drawLimb(userId, SimpleOpenNI.SKEL_LEFT KNEE, 
SimpleOpenNI.SKEL_LEFT_ FOOT); 


drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_RIGHT HIP); 
drawLimb(userId, SimpleOpenNI.SKEL_RIGHT HIP, 
SimpleOpenNI.SKEL_RIGHT KNEE); 

drawLimb(userId, SimpleOpenNI.SKEL_RIGHT KNEE, 
SimpleOpenNI.SKEL_RIGHT FOOT); 


strokeWeight (1); 


void drawLimb(int userId, int jointType1, int jointType2) { 


PVector jointPos1 = new PVector(); 
PVector jointPos2 = new PVector(); 
float confidence; 


// draw the joint position 

confidence = kinect.getJointPositionSkeleton(userId, jointType1, 
jointPos1); 

confidence = kinect.getJointPositionSkeleton(userId, jointType2, 
jointPos2); 

stroke(255, 0, 0, confidence * 200 + 55); 

line(jointPos1.x, jointPos1.y, jointPos1.z, jointPos2.x, jointPos2.y, 
jointPos2.z); 


Simple-OpenNI Callbacks 


The Simple-OpenNI callbacks are the same functions you used in the previous chapter. The only point 
to notice is that you update the userID integer to the last user you detected in the onNewUser() function. 


Speaking of classes and member visibility, you should add the keyword “public” before every 
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callback function. This is unnecessary in Processing, but if you are using another IDE, such as Eclipse, 


you will need to set all the callbacks as public or Simple-OpenNI won’t invoke them. 
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public void onNewUser(int userId) { 
println("onNewUser - userId: " + userId); 
println(" start pose detection"); 
kinect.startPoseDetection("Psi", userId); 
userID = userId; 


} 


public void onLostUser(int userId) { 
println("onLostUser - userId: " + userId); 


public void onStartCalibration(int userId) { 
println("onStartCalibration - userId: " + userId); 


public void onEndCalibration(int userId, boolean successfull) { 
println("onEndCalibration - userId: " + userId + ", successful: 
if (successfull) { 
println(" User calibrated !!!"); 
kinect.startTrackingSkeleton(userId) ; 


+ successfull); 


else { 
println(" Failed to calibrate user !!!"); 
println(" Start pose detection"); 
kinect.startPoseDetection("Psi", userId); 
} 
} 


public void onStartPose(String pose, int userId) { 
println("onStartdPose - userId: " + userId + ", pose: " + pose); 
println(" stop pose detection"); 
kinect.stopPoseDetection(userId) ; 
kinect.requestCalibrationSkeleton(userId, true); 


} 


public void onEndPose(String pose, int userId) { 
println("onEndPose - userId: " + userId + ", pose: " + pose); 


Summary 


You were introduced to a broad range of topics in this chapter. You learned how to implement three- 
dimensional skeleton tracking and use it to perform custom gesture recognition. You also had a 
refresher on object-oriented programming, and you learned how to write a new class from scratch. 

On the hardware side of things, you built Arduino-based autonomous devices, each with its own 
power source and wireless module, which permitted the communication of the units with your 
computer. You also learned about the RGB color space and that you can reproduce the whole color 
spectrum with only three RGB LEDs (Figure 7-27). You also learned how to calculate the appropriate 
resistors for the LEDs. In the end, you were able to control a whole lighting system with body gestures 
and make it aware of your position in space so it could respond by lighting the closest lamps to you with 
a higher intensity. 
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Figure 7-27. RGB lamp color change 


You can think of this chapter as a general introduction to building any sort of application for 
intelligent houses. Imagine applying this sort of gesture control to any of your home appliances. The 
introduction of the XBee wireless module adds a new set of possibilities for using Arduino remotely, and 
you will explore these possibilities in future chapters of this book. 
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Kinect-Driven Drawing Robot 


by Przemek Jaworski 


In this chapter, you will be using some of the techniques that you learned in previous chapters and 
exploring them further with a different goal in mind. You will be using servos to build a robotic arm that 


is able to draw algorithmic patterns on paper (Figures 8-1 and 8-2). 


Figure 8-1. Visualization of the installation 


167 


CHAPTER 8!) KINECT-DRIVEN DRAWING ROBOT 


Figure 8-2. Drawing robot in action 


Why an arm and not a two-axis pen plotter? Actually, an arm-like robot is simpler to construct and 
much easier to code. As in Chapter 6, you’re going to use servo motors, which are less complicated than 
steppers (no need for endstops and other sensors). In the course of this chapter, you will use Firmata, a 
library that allows you to control your Arduino board from Processing without the need to write any 
Arduino code. 

Your robot will be controlled by a Kinect-based tangible table, through which you will be able to 
issue instructions to the robot by moving your hands. It will be as simple as pointing your finger to an 
area and your robot will follow, drawing either your finger strokes or some sort of pre-defined 
algorithmic pattern. Table 8-1 lists the required parts and Figure 8-3 shows them. 


Table 8-1. The Parts List 


Part Description Price 
Screws, medium M4 screws, 25 mm length, and nuts $5 
Screws, small M3 screws, 20mm length, and nuts $3 
Klik construction system Choose the Starter Pack with a mix of various sized plates $12 
Power Supply Optional 5V, could be hacked ATX one $34.99 
Three servo motors Hitec HS-311 or similar $7 
eac 
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Part Description Price 
Extension lead cables Three for each servo $1 
Paper clips To attach the device to the table $5 for 
pack 
Two right angle metal brackets | To mount the robot $1 


Figure 8-3. Some parts shown graphically 


The parts taken from a slotted-plate construction system are 13 large plastic plates, 14 mid-sized 
plates, and 14 small joints (to make the structure more rigid). Apart from three servos (standard 0-180 
degree servos, similar to those used in earlier exercises), you'll need six small screws to assemble the 
arms to the servo horns (they’re usually supplied with the servos) and 14 short M4 screws (16 or 20mm 
length). Paper clips will be used to fasten the pen to the robot and the base to the table. 
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Building the Robot 


Before you start, a few words must be said about the construction. I used a slotted-plate construction kit 
(Klik - available from web sites like http: //www.mindsetsonline.co.uk). You can use any similar system 
consisting of perforated plates that can be connected at straight angles or by screws. The robot itself is 
actually quite simple. It consists of three moveable parts: a base that holds Arduino and one servomotor, 
and three parts constituting the arms. 

The construction doesn’t really have to look exactly as shown in Figure 8-4, as there are many ways 
of assembling a similar robot. Parts can be designed differently, even 3D printed or CNC machined. I 
leave this to your creativity. Just note that I'll be describing to a slotted-plate construction system. 
Optionally, you could make all required parts out of plastic or Perspex. 

The most important thing is to build a steady base with one servo (it will move the rest of the 
mechanism, so it has to be quite sturdy) and make sure it can be securely attached to the table. The 
other three parts, shown in Figure 8-4, should be made as light as possible; this will make it easier and 
faster to move. 


Pen Arduino 
part3 


part 1 part 2 


Servo 2 Servo 3 
Servo 1 


Figure 8-4. Construction of the arm 


Perhaps it’s best to start with easiest elements, so let’s focus on the arm at the end of the robot 
(Part 1). 


Part 1 


Part 1 is the part that holds the pen, so it’s the lightest part of all. The assembly drawing in Figure 8-5 
shows the elements and their contact points for Part 1. Use two M4 screws to fasten two plates together 
and two small M3 screws to attach it to servo horn. The bracket needs to be attached to the servo via the 
screw that comes with it. Figure 8-6 shows the finished part. 
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Figure 8-5. Part 1 assembly 


Figure 8-6. Part 1 finished 


Part 2 


Part 2 is a bit more complex as it includes an assembly of plates that hold the servo that connects Part 2 
to Part 1. The assembly should look like Figure 8-7. 
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Figure 8-7. Part 2 assembly 


Note that the top part is a set of two mid-sized plates, and not one large plate. They both stiffen the 
end so the piece can hold the servo attached to the front, as shown in Figure 8-8. The servo is attached 
with four standard M3 screws and one central screw. 


Figure 8-8. Part 2 finished 


Part 3 


Part 3 is the one closest to the base. Its construction is very similar to Part 2. The only difference is that it 
contains one more large plate at the bottom, additionally stiffening the structure (Figure 8-9). This plate 
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is fitted here because this part is positioned higher above the table than other parts, so there is enough 
clearance to do so. 


Figure 8-9. Part 3 assembly 


To connect the servo to the previous part, note that the spacing between the holes of the 
construction plates and those of the servo screws are different. You can drill new holes in the 
construction set or you can attach the servo to the arm using rigid wire (tightening it with pliers and 
rotating the ends spirally); see Figure 8-10. If you laser-cut your own parts, the holes will align, so you 
can simply use screws. 


Figure 8-10. Part 3 finished 
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The Base 


The base consists of four large plates and four small plates holding the servo in the middle. Additionally, 
there are two sets of three small plates joined in a corner shape to increase the rigidity of the part. Each 
corner shape is positioned at one side of the base (Figure 8-11). 


Figure 8-11. Base assembly 


The Arduino board can be attached by screwing it to the plastic parts using the mounting holes in 
the PCB and one M4 screw and nut (Figure 8-12). 


Figure 8-12. Arduino board assembled to the base 


Finally, attach the base to a small pad, which you then clip to the table (Figure 8-13). This way the 
robot stays in place, but you can move it easily if needed. 
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Figure 8-13. Assembled robot 


Building the Circuit 


The wiring of this project is quite simple as you are only using three servos. Just connect each servo’s 
GND connection with GND on Arduino, +5V wire to +5V on Arduino, and the signal wires to pins 9, 10, 
and 11 (see Table 8-2 and Figure 8-14). The positive voltage wire (+5V) is usually a red one, ground is 
black, and the signal wire is orange or yellow. 


Table 8-2. Servo Connections 


Servo 1 Servo 1 Servo 1 Servo 2 Servo 2 Servo 2 Servo 3 Servo 3 Servo 3 
Ground +5V Signal Ground +5V Signal Ground +5V Signal 


Arduino Arduino Arduino Arduino Arduino Arduino Arduino Arduino Arduino 
GROUND | 5V Pin 9 GROUND | 5V Pin 10 GROUND = 5V Pin 11 
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Figure 8-14. The circuit (signal wires connect to pins 9, 10, and 11 at the top.) 


If you have a +5V power supply that can provide more than 0.5A, simply connect it to Arduino using 
the built-in socket. This is only necessary if your computer/laptop USB port is not able to meet the 
demand. In some newer computers, USB ports are powerful enough to drive three small servos without 
a problem. 


Testing the Circuit 


Next, test the servos. In previous projects, you learned how to drive your servos using Arduino code and 
how to control their positions using serial communication from Processing. In this project, you will take a 
different approach: you will utilize a Processing library to manipulate the servos directly from Processing. 


Firmata and the Arduino Library for Processing 


The Arduino library is one of Processing’s core libraries so you don’t need to install it. This library 
communicates to a firmware installed in your Arduino board called Firmata, eliminating the need to 
write any separate Arduino code to drive your servos. 

As opposed to previous examples, everything is in one piece of code in this project. This approach is 
very advantageous for some uses but is not applicable to freestanding Arduino projects. Arduino can’t be 
used without a computer if you are using this library, so it always needs to be connected to the computer 
via USB cable or other wireless means. If the library doesn’t exist by default on your Arduino IDE, you 
can download it from http://www. arduino.cc/playground/Interfacing/Processing. 

Before you start testing the circuit, you must upload the Firmata code to Arduino so it’s ready to 
listen to the instructions via serial port. Open Arduino environment and navigate to File » Examples > 
Firmata > Servo Firmata. Make sure you have the proper board and COM port selected, and press the 
Upload button. After the successful upload, you can write your first servo navigation program. 


Servo Test 
Open the Processing environment and type the following code: 


import processing.serial.*; 
import cc.arduino.*; 
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Arduino arduino; 
int servoPin = 11; // Control pin for servo motor 
void setup(){ 
size (180, 100); 
arduino = new Arduino(this, Arduino.list()[0]); 
arduino.pinMode(servoPin, Arduino.OUTPUT) ; 


} 


void draw(){ 
// the servo moves to the horizontal location of the mouse 
arduino.analogWrite(servoPin, mouseX); 


} 


Note that you’re using pin 11 for connecting the servo, which is a PWM pin. You're also not using 
any COM ports in this program. Instead you're asking the computer for a list of available Arduinos (there 
might be more than one) and specifying which one you're using. That’s what Arduino. list()[0] means. 
However, sometimes it detects other devices connected to COM ports as Arduinos, so depending on 
your setup, you might have to use Arduino. list()[1] instead. 

Once you run this simple Processing application, your servo should rotate as you move your mouse 
within the app’s window. The servo accepts angles from 0 to 180; hence the 180-pixel width used. 

Upon successful testing of this simple setup, you can move on to construct the kinetic part of the 
installation. The difference between the previous example and your final piece is that you’re going to be 
using three servos connected to PWM pins 9, 10, and 11. (Depending on your Arduino version, you 
might have more PWM pins. It’s easy to recognize them by a small white PWM mark). All GND wires 
should go to GND on Arduino and all power wires to +5V. 


Robot Simulation 


The main routine of the installation is the following: you’re going to scan a tangible table area, detect a 
hand (or a pointer) over the table, filter the point cloud, and extract the X and Y coordinates of where the 
robot should move. These X and Y values will be translated into servo angles in order to make the robot 
move to the same location. Let’s have a look at the geometry of your robot. 

You have three modules, each driven by separate servo and each able to rotate between 0 and 180 
degrees. Let’s assume the length of each module is exactly 150mm. This means you must translate these 
X and Y coordinates into angular values sent to each servo. This might look complicated, but fortunately 
you can use some simple geometric dependencies to find and use the angles. 

Look at the diagram in Figure 8-15. You can see that each X and Y coordinate can be translated into 
polar coordinates: length L and rotation angle. This is true for all points lying within maximum reach of 
the robot and within a 0-180 angle in relation to upper horizontal edge of your working area. 
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Figure 8-15. Robot angles 


This translation, however, poses a small problem. As you can see in the right drawing in Figure 8-15, 
sometimes (or most of the time) there are two ways of reaching a given position. The tip of the pen will 


reach XY point in both cases, so how do you decide which one to use? 


The truth is, that once you choose one side, you can’t reach all the points in your working area 
simply because your robot is hitting the limits of its arm’s servo rotation. Each servo can only rotate from 
-90 to 90 degrees (or 0 to 180, depending on reference angle), so the pen’s closer position to the base will 


be reached by the robot bending into a C shape. 


For the purpose of this exercise, you’re going to simplify the problem. You'll assume that your robot 
is always using only one side. To do that, you’re going to rotate the base by 45 degrees, as shown in 


Figure 8-16. 


Figure 8-16. Robot span (example setup) 
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This way, the working area resembles a slice of the circle with its minimum and maximum reach 
arcs cut off at the top and bottom (Figure 8-17). Of course, you could use a more intelligent algorithm 
allowing use of more space, but to keep things uncomplicated, just stick to this setup. 


minimum 


maximum 


Figure 8-17. Robot’s working area 


Angle Measuring Program 


Finally, you must tackle the translation from Cartesian coordinates (XY) to polar coordinates 
(angle/ distance). Knowing the X and Y and your origin of coordinates (the pivotal point of the first 
segment’s servo), you can easily calculate the distance to the pen’s desired X and Y position using 
Processing’s dist() function. The angle doesn’t pose a big challenge because it’s usually derivable from 
the arcsine. The functions return an angle based on the proportion of the sides of a triangle formed by 
the XY point, the origin (0,0) point, and its projections on the axis of the coordinate system. In this case, 
your angle will be arcsine(Y coordinate/total length L), assuming the base is at the (0,0) point. 

To make more sense of it, let’s test the angle-measuring program. 


void setup(){ 
size (800, 600); 


void draw(){ 
background(255); 


First, center your X coordinate in the middle of the screen. 
translate(width/2, 0); 
Then, capture the coordinates of your pen tip (mouse). 


float penx 
float penY 


mouseX - width/2; 
mousey; 


ellipse(penX, penY, 20, 20);//draw pen tip 
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ellipse(0, 0, 20, 20); 
line(0, 0, penX, penY); 
float len = dist(0, 0, penX, penY); //let's measure the length of your line 


The asin() function returns an angle value in a range of 0 to PI/2, in radians. The following line 
makes sure that the angle is greater than PI/2 (90 degrees) when penx is negative: 


float angle = asin(penY/len); 
if (penX < 0) { angle = PI - angle; } 


Then output the angle value converted from radians to degrees. 


println("angle = " + degrees(angle)); 
println("length = " + len);//print out the length 


And finally, draw your angle as an arc. 
arc(0, 0, 200, 200, 0, angle); 
This demonstrates how to extract any angle from the proportions of a triangle defined by three 
points. But the goal is to find not one but three angles, one for each servo; therefore you need to go a bit 


further. If you look at Figure 8-18, you can see how to perceive the virtual model of the robot. This 
should make it much easier to understand the math behind it. 


angle 


Figure 8-18. Robot’s angles (assuming no 45 degree rotation has been applied yet) 


Robot Simulation Program 
Let’s imagine that the length of your reach (described previously as L) can be split into three 


components, being perpendicular projections of each sub-arm onto a straight line. They’re called d’, d, 
and d’ again (the first and last ones are the same). Using this method, you can codify each polar 
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coordinate (angle and length) as an angle and two different lengths (d’ and d). But wait a minute; don’t 
you need the angles, not the lengths, to drive the servos? 

That’s right. Therefore, having d’ and d, you can use their proportion, and the asin() function again, 
to determine it! 

So, the length of your reach is L. 


L= dist(0, 0, penx, penY) 

But also, looking closely at Figure 8-18, you can determine that 
L=ad’+d+d=2*d’ 4d 

D is given (150mm in your robot), so 


d’ = (L-—150mm) / 2 


Therefore your angle (a) is 
a= acos(d’/d) = acos(d’/150) 
Voila! So now you have all you need: the polar coordinates of the XY point (angle and length) and 


the internal angles of the trapezoidal shape formed by your arms. Using that, you can deduct the 
following: 


¢ Angle of first servo: main angle + a 
¢ Angle of second servo: 90-a 
¢ Angle of third servo: 90 -a 


Note that these angles are relative to the position of each servo. 

You are going to build this sketch from the Angle Measuring sketch (described before), so add the 
following code at the very end of draw() function, just before the closing curly bracket. 

First, constrain the length to the physical size of the robot’s parts. 


450; } 
150; } 


Then, use what you have just learned to calculate your three servo angles. 


if (len > 450) { len 
if (len < 150) { len 


float dprime = (len - 150) / 2.0; 
float a = acos(dprime / 150); 


float angle1 = angle + a; 
float angle2 = -a; 
float angle3 = -a; 


And finally, use the angles to draw the robot on screen. 


rotate(anglet) ; 
line(0, 0, 150, 0); 
translate(150, 0); 


rotate(angle2); 
line(0, 0, 150, 0); 
translate(150, 0); 


rotate(angle3); 


line(0, 0, 150, 0); 
translate(150, 0); 
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After running it, you should see the image shown in Figure 8-19. 


Figure 8-19. Screenshot from the program 


The full code for this exercise, declaring the angles as global variables, is as follows: 


float angle1 = 0; 
float angle2 = 0; 
float angle3 = 0; 


//servo robot testing program 
void setup(){ 
size (800, 600); 


void draw(){ 
background(255) ; 
translate(width/2, 0); //you need to center your X coordinates to the middle of the screen 
float penX = mouseX - width/2; //you're capturing coordinates of your pen tip (mouse) 
float penY = mouseY; 
ellipse(penX, penY, 20,20); 
ellipse(0,0, 20,20); 
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line(0,0, penX, penY); 

float len = dist(0,0, penX, penY); //let's measure the length of your line 

float angle = asin(penY/len); //asin returns angle value in range of 0 to PI/2, in radians 

if (penx<o) { angle = PI - angle; } // this line makes sure angle is greater than PI/2 (90 
// deg) when penX is negative 

println("angle = " + degrees(angle)); //you're outputting angle value converted from radians 
// to degrees 

println("length = " + len);//print out the length 

arc(0,0,200,200, 0, angle); //let's draw your angle as an arc 

if (len>450) len = 450; 

if (len<150) len = 150; 


float dprime = (len - 150)/2.0; 
float a = acos(dprime/150); 
angle1 = angle + a; 

angle2 - a3 

angle3 - a3 


rotate(angle1) ; 
line(0,0,150,0); 
translate(150,0); 


rotate(angle2); 
line(0,0,150,0); 
translate(150,0); 


rotate(angle3); 
line(0,0,150,0); 
translate(150,0); 


The simulation shows the position of the robot on screen. Notice that the len variable is limited to a 
range of 150 to 450 in order to be physically reachable. This just makes sure that you’re working in 
realistic working area. 

This concludes the virtual testing phase. Let’s get back to the wires and the Arduino. 


Driving the Physical Robot 


You should have your robot built. The Firmata code should be uploaded to the Arduino board and the 
three servos should be connected to pins 9, 10 and 11. Using the servo-testing program, make sure that 
servoipin is the one connected to Part 1, servo2pin to Part 2, and servo3pin to Part 3 and the base. 

Add the Serial and Arduino libraries to your previous code, and use them to send the angles to the 
servos through the Arduino board. 


import processing.serial.*; 
import cc.arduino.*; 
Arduino arduino; 


int servoipin = 11; 
int servo2pin = 10; 
int servo3pin = 9; 
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float angle1 
float angle2 
float angle3 
void setup(){ 
size (800, 600); 

arduino = new Arduino(this, Arduino.list()[0]); 
arduino.pinMode(servoipin, Arduino.OUTPUT) ; 
arduino.pinMode(servo2pin, Arduino.OUTPUT) ; 
arduino.pinMode(servo3pin, Arduino.OUTPUT) ; 


} 


void draw(){ 
background(255); 
translate(width/2, 0); 


0; //declaration of all 3 angles for servos 
0: 
0; 


float penx 
float penY 


mouseX-width/2; 
mousey; 


ellipse(penX, penY, 20,20); 
ellipse(0, 0, 20, 20); 


line(0, 0, penX, penY); 

float len = dist(0, 0, penX, penY); //let's measure the length of your line 
float angle = asin(penY/len); 

if (penX < 0) { angle = PI - angle; } 


println("angle = " + degrees(angle)); //output angle in degrees 
println("length = " + len);//print out the length 
arc(0,0,200,200, 0, angle); //let's draw your angle as an arc 
if (len > 450) { len = 450; } 

if (len < 150) { len = 150; } 


float dprim = (len - 150)/2.0; 
float a = acos(dprim/150); 


angle3 = angle + a; 
angle2 = -a; 

angle1 = -a; 
rotate(angle3); 


line(0, 0, 150, 0); 
translate(150, 0); 


rotate(angle2); 
line(0, 0, 150, 0); 
translate(150, 0); 


rotate(angle1); 


line(0, 0, 150, 0); 
translate(150, 0); 
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Use the Arduino library to send the values of the angles to the Arduino board. 


arduino.analogWrite(servoipin, 90-round(degrees(angle1))); // move servo 1 
arduino.analogWrite(servo2pin, 90-round(degrees(angle2))); // move servo 2 
arduino.analogWrite(servo3pin, round(degrees(angle3))); // move servo 3 


These last three lines are the instructions to move the servos. Due to some differences between 
graphical interpretation on screen and the way the servos operate, you must add some modifications. 
First, you must convert angles from radians to degrees, but you also must change the values by making 
them negative (flipping direction) and adding a half-rotation. This is because the middle of the angular 
scope of rotation is not 0 (then the scope would be from -90 to 90 degrees), but 90 (as it goes from 0 to 
180). The exception from this rule is your base servo, which takes the same angle as in the graphic 
representation. 

After connecting the Arduino and launching this program, you'll see all the servos straighten up to 
one line. Then they start following the on-screen geometry. Try to use gentle movements; otherwise the 
servos will move abruptly and could unfasten the assembly if it’s slightly weak. Generally, it’s a good idea 
to calibrate the servos first by dismantling the sub-arms and connecting them again once the servos get 
a signal from Arduino (without the program running). They are usually set to 90 degrees automatically, 
so once you secure the robot in straight position, they retain this as starting point. 

After some experimenting, depending on your mechanical setup, you might want to fine-tune the 
way the robot applies angles to joints. It’s best to do this by using multipliers on angles (shown in bold 
for emphasis). 


arduino.analogWrite(servoipin, 90-round(degrees(angle1*1.25))); // move servo 1 
arduino.analogWrite(servo2pin, 90-round(degrees(angle2*1.35))); // move servo 2 
arduino.analogWrite(servo3pin, round(degrees(angle3))); // move servo 3 


It’s a matter of trial and error, and some common sense, to find the best values. The goal is to match 
the orientation of the robot with what you see on screen. 

After verifying that everything works, use a paper clip to attach the pen to the furthest sub-arm. Now 
you can use your mouse to express your creativity on paper! 


Kinect: Tangible Table Interface 


To make the installation complete, you need to add the sensing part. It will allow you to use gestures to 
control the drawing or, you could also say, make your robot interpret your posture and draw something 
on its own. 

You then need to add another layer of computational mechanisms, the one that interprets gestures 
and translates them into a set of XY coordinates to pass to the robot. In this way, you connect the two 
sides together: the Kinect scans your hand or finger, extracts XY coordinates from it, and passes it on to 
the robot simulation Processing sketch, which converts the data into a series of servo rotations. 

The idea of extracting gestures or coordinates from your hands and fingers is not that simple, 
though, because the orientation of the point cloud is a bit tricky in relation to the table. You could, of 
course, hang the Kinect from the ceiling, make sure it’s perpendicular to the table, and align it with your 
(0,0) point, which is the base of the robot. In practice, however, this could be hard to realize; the 
orthogonal aligning, precision, and installation of the sensor are quite complicated tasks (and drilling 
holes in the ceiling might not be the best idea). 

You're going to pursue a slightly different path. You’ll assume that the Kinect is somewhere in space, 
in any orientation, and you will try to fine-tune it with the scene inside the program. In practice, this is a 
much more flexible approach and less prone to errors than other techniques. 
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To make things easy, use a tripod to mount the Kinect and make it point to the middle of your 
working area. Make sure the Kinect is in a horizontal position, only tilting towards the table (not tilting 
left or right), as shown in Figure 8-20. 


Figure 8-20. Tangible table setup; scanning volume is shown with dotted lines. 


After positioning the Kinect on the tripod, open the DepthMap3d example from the Simple-OpenNI 
examples and look at the point cloud. You will see that the surface of the table is at an angle with the 
Kinect’s coordinate system, making it difficult to extract coordinates from your position in space (Figure 
8-21). With the example running, you can rotate the point cloud to see that, when viewed at a certain 
angle (from the side), the table is indeed angled. Moreover, the coordinate system to which these points 
refer is related to the Kinect itself; the (0,0,0) point is the sensor’s camera. The task is to make the point 
cloud coexist in the same coordinate system as your robot. 
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Figure 8-21. Kinect uncalibrated point cloud 


Calibrating the Point Cloud 


You need to align the table surface to your coordinate system by transforming the Kinect point cloud 
coordinates. What does this mean? 

You must virtually rotate the table (tilt it) so it stays perpendicular to the Kinect axis (therefore being 
in the XY plane; see Figure 8-22). Also, you must shift it a bit so the origin (0,0) point matches your 
robot’s origin of coordinates. 

Once the point cloud has been aligned, the z-coordinate (or depth) of each point allows you to filter 
the point cloud and extract only the relevant data. In your case, this means the finger position, whenever 
it is close enough to the table. 
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pivot point 


Z axis (X and Y = 0) 


table surface depth 
est __ _—_ 


Figure 8-22. Rotating the point cloud 


This might seem a little complicated at first, but the effort is worth it as you can now fine-tune the 
calibration with program variables, which is not possible in an orthogonal, mechanically fixed setup (or 
it requires moving the sensor). 

Figure 8-22 illustrates the strategy: after rotating the table (point cloud) to the perpendicular 
position, you get the Z axis and mark all points that have X and Y = 0. What’s most important, though, is 
that you can now filter the point cloud and cut out unnecessary bits like the background. 

How do you do that? You won’t use conventional Processing rotations with matrices such as 
rotateX(). Rotations and translations with standard 3D matrices keep the coordinates of each point 
unchanged. This means that even though the point (or object) appears in completely different place, it 
still has the same X, Y, and Z coordinate. It’s more like moving coordinate system around, together with 
all its contents. 


Rotation Equations 


Back in the old days when there was no hardware graphics acceleration or OpenGL, there was no way of 
using glTranslate and glRotate to draw scenes in 3D. Coders working on 3D interactive games had to 
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create mathematical descriptions of their 3D worlds by themselves. Even in 2D games, there was a need 
to rotate things on screen. 

Well, to keep things simple, let’s have a look at equations they were using. If you get a point on plane 
(X, Y) and want to rotate it, you have to calculate it like this: 


angle = your rotation 
newx = X * cos(angle) — Y * sin(angle) 
newY = X * sin (angle) + Y * cos(angle) 


Yes, that’s it! If you want to understand these equations, don’t Google them, or you risk being scared 
to death by the hundreds of scientific papers about 3D rotations of geometric entities! The best way to 
find an explanation is to search for old game-writing tutorials or simple computer graphics books from 
the 90s (such as those by Bowyer and Woodwark or Foley and Van Damme). 

How does it work? Imagine you have a point on plane with coordinates (1, 0). If you want to rotate it, 
you multiply it with the previous operation and you get 


newx = 1 * cos(angle) 


newY = 1 * sin(angle) 


This is, in fact, an equation for a circle, where the angle is between 0 and 360 degrees. Using sin, cos, 
and the previous operations, you can rotate any two coordinates around a (0,0) point, getting in return 
true numerical values of its coordinates (which is what you need!). So, after this lengthy explanation, 
let’s put these concepts to use. 


Rotating the Point Cloud 


Let’s start coding another example from scratch. The following code is derived from standard 
DepthMap3D code found in Simple-OpenNI examples. It displays the point cloud and allows you to 
rotate it. Then, after rotating the view so you look at the table from the side, you virtually rotate the point 
cloud until the table stands in a perpendicular position to the Kinect. You then hardcode the rotation 
angle into your code so the program is calibrated. 

Import all the necessary libraries and initialize the Simple-OpenNI object. Then declare the 
variables that control the perspective. 


import processing.opengl.*; 
import SimpleOpenNI.*; 
SimpleOpenNI kinect; 


float zoomF = 0.3f; 
float rotX = radians(180); 
float rotY = radians(0); 


The float tableRotation stores your rotation angle. This is the value that needs to be assigned a 
default value for the calibration to be permanent. 


float tableRotation = 0; 


The setup() function includes all the necessary functions to set the sketch size, initialize the 
Simple-OpenNI object, and set the initial perspective settings. 


void setup() 
{ 


size(1024, 768, OPENGL); 
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kinect = new SimpleOpenNI(this) ; 

kinect.setMirror(false) ; // disable mirror 
kinect.enableDepth(); // enable depthMap generation 
perspective(95, float(width)/float(height), 10, 150000); 


Within the draw() function, update the Kinect data and set the perspective settings using the 
functions rotateX(), rotateY(), and scale(). 


void draw() 


kinect.update(); // update the cam 
background(255); 

translate(width/2, height/2, 0); 
rotateX(rotX); 

rotateY(rotY); 

scale(zoomF) ; 


Then declare and initialize the necessary variables for the drawing of the point cloud, as you have 
done in other projects. 


int[] depthMap = kinect.depthMap(); // tip - this line will throw an error if your Kinect 
// is not connected 

int steps = 3; // to speed up the drawing, draw every third point 

int index; 

PVector realWorldPoint; 


Set the rotation center of the scene for visual purposes to 1000 in front of the camera, which is an 
approximation of the distance from the Kinect to the center of the table. 


translate(0, 0, -1000); 
stroke(0); 


PVector[] realWorldMap = kinect.depthMapRealWorld(); 
PVector newPoint = new PVector(); 


To make things clearer, mark a point on Kinect’s z-axis (so the X and Y are equal to 0). This code 
uses the realWorldMap array, which is an array of 3D coordinates of each screen point. You’re simply 
choosing the one that is in the middle of the screen (hence depthWidth/2 and depthHeight/2). As you are 
using coordinates of the Kinect, where (0,0,0) is the sensor itself, you are sampling the depth there and 
placing the red cube using the function drawBox() (which you implement later). 


index = kinect.depthWidth()/2 + kinect.depthHeight()/2 * kinect.depthwWidth(); 
float pivotDepth = realWorldMap[index].z; 

fi11(255,0,0); 

drawBox(0, 0, pivotDepth, 50); 


When you finish the program and run it, you'll discover that the point on the z-axis hits the table 
and lands somewhere in the middle of it. If it doesn’t, move your Kinect and tripod so it hits the middle 
of the table. Feel free to adjust the tilt and the height of the sensor but don’t rotate it around the tripod; 
keep it perpendicular to the table. 

The red cube is a special point. It’s used to determine the axis of your rotation. To make it clearer, 
and accessible in the rest of the code, the local variable pivotDepth is assigned this special depth value. 

You are now ready to virtually tilt the table and display the tilted points. But which pair of 
coordinates should you use for rotation? You have three: X, Y, and Z. The answer is Y and Z. The X 
coordinate stays the same (right side of the table, which is positive X, stays on the right; the left side, 
negative X, on the left). 
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for(int y=0; y < kinect.depthHeight(); y+=steps) 
for(int x=0; x < kinect.depthWidth(); x+=steps) 


index = x + y * kinect.depthWidth(); 
if(depthMap[index] > 0) 


realWorldPoint = realWorldMap[ index]; 
realWorldPoint.z -= pivotDepth; 


float ss = sin(tableRotation); 

float cs = cos(tableRotation); 

newPoint.x = realWorldPoint.x; 

newPoint.y = realWorldPoint.y*cs - realWorldPoint.z*ss; 

newPoint.z = realWorldPoint.y*ss + realWorldPoint.z*cs + pivotDepth; 


point(newPoint.x, newPoint.y, newPoint.z); 


Now, display the value of your tableRotation. It tells you the value of the tilt, as you will be rotating 
it by eye. After you find the proper alignment, it must be written down and assigned to the variable at the 
beginning of the program (so the table stays rotated all times). 


println("tableRot = " + tableRotation); 
kinect.drawCamFrustum(); // draw the kinect cam 


} 


Within the KeyPressed() callback function, add code that reacts to the pressing of keys 1 and 2 by 
changing the values of tableRotation in small increments/decrements. Also, listen for input from the 
arrow keys to change the point of view. 


void keyPressed() 
{ 

switch(key) 

i 


case '1': 
tableRotation -= 0.05; 
break; 

case '2': 
tableRotation += 0.05; 
break; 


} 
switch(keyCode) 
{ 


case LEFT: 
rotY += 0.1f; 
break; 

case RIGHT: 
// zoom out 
rotY -= 0.1f; 
break; 

case UP: 
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if (keyEvent.isShiftDown()) 
zoomF += 0.02f; 


else 
{ 
rotX += 0.1f; 
} 
break; 
case DOWN: 
if(keyEvent.isShiftDown()) 


zoomF -= 0.02f; 
if(zoomF < 0.01) 
zoomF = 0.01; 
} 


else 
{ 
rotX -= 0.1f; 


} 


break; 


Finally, add a small, useful function that draws the box. Create another tab (call it functions, for 
example), and paste the following: 


void drawBox(float x, float y, float z, float size) 


pushMatrix(); 
translate(x, y, z); 
box(size); 

popMatrix(); 


Run the code and use the arrow keys (LEFT/RIGHT) to view the cloud from the side (meaning you 
rotate it graphically, without changing anything with coordinates). The table surface should appear as a 
thin line of tilted points. Then, using keys 1 and 2, you should be able to tilt the table (proper rotation, 
changing Y and Z coordinates) and find its best position. Aim to make it close to perpendicular to the 
Kinect axis, as per Figure 8-23. 
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Figure 8-23. Kinect calibrated point cloud 


And there you go! Your table is virtually rotated now, so it seems like the Kinect is looking straight at 
it. Using the output from Processing, hardcode the value of this rotation into the program. Simply change 
tableRotation at the beginning of the code to whatever the readout is on your console (mine was -1.0, 
but yours might be different). If you run the program again, you'll see the table straight from the top. 

You also need to hardcode the value of pivotDepth so when you move your hand on the table, the 
rotation pivot won’t change. Substitute this line in your code: 


float pivotDepth = realWorldMap[index].z; 
By this one: 
float pivotDepth = 875; 


In our case, the centre of our tangible table was 875mm away from the Kinect, but you will need to 
extract this value from your table rotation code. 
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Point Cloud Filtering 


As you look at the scene from the side, try to manipulate the objects on the table by looking at the 
screen. What’s visible? You can probably see some objects, your hands, and some background (like the 
floor). You should also see the table surface. The thing is, most of this is not necessary for your tracking. 
The only object you're interested in is the hand/pointer (anything that can be clearly distinguished from 
the rest of the point cloud). 


BD robot tabie2 | 


cut-off depth 


= 


background noise 


table surface 


pointing hand 


elements of the robot 


Figure 8-24. Point cloud filtering 


Take a look at Figure 8-24. It clearly shows how the point cloud is structured and which depths you 
can ignore. By simply adding an if() statement around the point drawing function, you can get rid of 


most of the unnecessary data. 
First, declare the zCutOffDepth global variable at the top of your sketch. 


float zCutOffDepth = 1200; 


Then, add a conditional wrapper around the point drawing function. 
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if (newPoint.z < zCutOffDepth) { 
point(newPoint.x, newPoint.y, newPoint.z); 


To know the value in runtime, add following line at the very end of the draw() function (before the 
closing curly brace): 


println("cutOffDepth = " + zCutOffDepth) ; 


This reads the value and hardcodes it for later. To be able to change this cut-off depth at any time, 
add two more cases into the keyPressed() function, inside the switch(key) statement. 


case '3': 
zCutOffDepth -= 5; 
break; 

case '4': 
zCutOffDepth += 5; 
break; 


Run the code with these additions. Viewing the point cloud sideways, press keys 3 and 4 until you get 
rid of all the points at the bottom of the table. In fact, you’re also going to cut off the robot itself, leaving 
only some space about 100mm above the table. After performing this, you should see nothing—just a 
pure white screen. But as soon as you hover your hand above the table, it should appear as black points. 

As before, once the calibration is done, you should hardcode the value of zCutOffDepth at the 
beginning of the program. In my setup, it was something around 700mm, but yours might be different, 
depending on how close the Kinect is to the table. 


Finding the Finger Position 


Now let’s get back to the main idea: extracting the X and Y coordinates of the pointer and passing them 
on to the robot-driving mechanism (which already knows how to translate Cartesian coordinates to 
three separate servo angles). 

To get the X and Y coordinates, you will use a trick to find a special point in the entire point cloud: 
your finger tip. In a typical case, it’s the Jowest point above the table, assuming you’re considering the 
hand only. Usually you hold your hand in such a way that the finger is the object closest to the surface 
(or its tip, actually). Therefore, your task is simply to find the lowest point in scanned area. 
Mathematically speaking, this is the point with highest Z-coordinate. 

To make things clearer and easier to understand, I will first describe the changes necessary to 
complete this task and then I will include the entire code of the draw() function so you can see the 
changes in context. 

Previously, you filtered the point cloud using the depth value and removed points that were too far. 
Now the time has come remove all the points that are outside of your working area (which is a rectangle). 

To filter the cloud a bit more, you must draw your reference area first. I assume this is 800 x 600 mm 
(it would be useful to place a sheet of paper with these dimensions on the table). The Kinect should be 
pointing to the middle of it, which will be your (0,0) point. Therefore, to draw it, you need these four 
lines right after the point drawing loop (be careful to type the coordinates properly). 


line(-400, -300, pivotDepth, 400, -300, pivotDepth); 
line(-400, 300, pivotDepth, 400, 300, pivotDepth); 
line( 400, -300, pivotDepth, 400, 300, pivotDepth); 
line(-400, -300, pivotDepth, -400, 300, pivotDepth); 


You are using pivotDepth as a z-coordinate so the lines are drawn on the table. After running the 
program, you should see a rectangle at table depth. 
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To clean things up a bit more, let’s filter out points that are outside of this area. So, in the condition 
that currently resides right before the point-drawing command 


if (newPoint.z < zCutOffDepth) { 
point(newPoint.x, newPoint.y, newPoint.z); 


change it to 


if ((mewPoint.z < zCutOffDepth) && 
(newPoint.x < 400) && 
(newPoint.x > -400) && 
(newPoint.y < 300) && 
(newPoint.y > -300)) { 


point(newPoint.x, newPoint.y, newPoint.z); 


This will get rid of all the points outside of the table boundary. Run the program; you should see a 
rectangle with red box in it (in the middle of the table). If you hover your hand above the work area, you 
should be able to see it on screen (and nothing else; once the zCutOffDepth variable is set properly, it 
should cut out unnecessary noise). 

Since the cloud is filtered, you’re left with just the space directly above the table. If there are no 
objects in this space, you receive zero points from the scan. Once you put something there (your hand or 
a stick), it should appear. Knowing this, you can safely add more filtering conditions, the ones you 
require to find the lowest point above the table. 

Let’s declare a special variable, something that will store your target as a point, so all three 
coordinates are known. This should be at the beginning of the code, so it’s defined as a global coordinate. 


PVector pointer = new PVector(); 


Then, for each loop, you have to reset the maximum depth value found (the depth of the furthest 
point in Z direction you’ve found each time). This is reset just before your main scanning loop, right 
after the drawBox(0,0, pivotDepth, 50); line. 


float maxDepth = 0; 


To find the point, go to the core of the scanning loop and add a conditional statement just after the 
instruction that draws each point. This statement checks if the point you're drawing is the furthest one 
(closest to the table). 


if (newPoint.z > maxDepth) 


Then it stores its value for later, along with the depth value encountered. In other words, it stores two 
things: the maximum depth found during scanning (the point closest to the table), and the x, y, and z 
coordinates of this point. 

To summarize, these steps are highlighted in bold in the following block of code, which is the main 
scanning loop in its entirety: 


float maxDepth = 0; 
for(int y=0;y < context.depthHeight();y+=steps) 


for(int x=0;x < context.depthwWidth() ;x+=steps) 


{ 
index = x + y * context.depthWidth(); 
if(depthMap[index] > 0) 


realWorldPoint = realWorldMap[ index]; 
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realWorldPoint.z -= pivotDepth; //subtract depth from z-coordinate 


float ss = sin(tableRotation); 

float cs = cos(tableRotation); 

newPoint.x = realWorldPoint.x; //x doesn't change 

newPoint.y = realWorldPoint.y*cs - realWorldPoint.z*ss;//rotate Y 
newPoint.z = realWorldPoint.y*ss + realWorldPoint.z*cs;//rotate Z 


newPoint.z += pivotDepth; //add depth back again 


if ((nmewPoint.z < 
(newPoint.x < 
(newPoint.x > 
(newPoint.y < 
(newPoint.y > 


zCutOffDepth) && 
400) && 

-400) && 

300) && 

-300) ) 


point(newPoint.x,newPoint.y,newPoint.z); //draw point! 
if (newPoint.z>maxDepth) //store deepest point found 


maxDepth = 


pointer.x 
pointer.y 
pointer.z 


newPoint.z; 
newPoint.x; 
newPoint.y; 
newPoint.z; 
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Finally, after the loop has been executed, you can draw the deepest point as a yellow box by adding the 
following code right after the loop: 


£111(255,255,0); 


drawBox(pointer.x,pointer.y,pointer.z,50); 
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Figure 8-25. Finding the finger position 


The yellow box now shows the tip of one of the fingers on the pointing hand. Because all this 
information is stored in the pointer variable, you can easily retrieve it and direct the robot with it. 

You should see something resembling Figure 8-25. The yellow box might be a little bit shaky, but it 
should always stick to the lowest (closest to the table) part of the point cloud. If you see parts of the table 
or parts of the robot, it means your zCutOffDepth is miscalibrated. If so, return to the calibration part of 
the “Point Cloud Filtering” section and fine-tune this value until you get clean scan of the space above 
the work area. 


Virtual Robot Model 


Before you connect the robot to this piece of code, you need to make sure it works. Let’s start with a 
virtual version of it. As before, you determine the angle based on XY coordinates; however, you need to 
align both coordinate systems together. The point cloud’s coordinate system has its center where the red 
box is and you need it aligned with the robot’s main pivot point. Knowing that the robot’s center point 
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needs to be reconfigurable, let’s declare it as two variables. At the end of the draw() loop, add the 
following code: 


float robotX 
float robotY 


0; 
-300; 


pushMatrix(); 
translate(0,0, pivotDepth) ; 
ellipse(robotX, robotY, 30,30); 
ellipse(pointer.x, pointer.y, 30,30); 
line(robotX, robotY, pointer.x, pointer.y); 
popMatrix(); 


You use pushMatrix() and popMatrix() to make sure you’re drawing in the table’s depth. By using 
translate(0,0, pivotDepth) you’re pushing your drawing plane; from now on, your Z depth is equal 
zero at table’s surface. This makes the task of drawing on it simpler as you can use two-dimensional 
commands like ellipse(). 


Figure 8-26. The finger position projected on the table 
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What you see in Figure 8-26 is a projection of the tip of the pointer (made by projecting it on the 
table) connected with the virtual robot’s center. This is exactly what you were doing at the beginning, so 
now you can easily copy and paste the previous code and use it to detect the angles. You should be able 
to run your code and see something similar. 

To draw entire virtual robot with its sub-arms, the last block of code in the draw() function can be 
modified to look like the following: 


float robotX = 0; 
float robotY = 


pushMatrix(); 
translate(0, 0, pivotDepth) ; 
ellipse(robotX, robotY, 30, 30); 
ellipse(pointer.x, pointer.y, 30, 30); 
line(robotX, robotY, pointer.x, pointer.y); 


float penX = pointer.x; //you're capturing coordinates of your pen tip (mouse) 

float penY = pointer.y; 

float len = dist(robotX, robotY, penX, penY); //let's measure the length of your line 

float angle = asin((penY-robotY)/len); //asin returns angle value in range of 0 to PI/2, 
// in radians 

if (penx<o) { angle = PI - angle; } // this line makes sure angle is greater than PI/2 
// (90 deg) when penX is negative 

println("angle = " + degrees(angle)); //you're outputting angle value converted from 
// radians to degrees 

println("length = " + len);//print out the length 

arc(robotX, robotY, 200, 200, 0, angle); //let's draw your angle as an arc 

if (len > 450) { len = 450; } 

if (len < 150) { len = 150; } 


float dprime = (len - 150) / 2.0; 
float a = acos(dprime / 150); 
float angle3 = angle + a; 

float angle2 = - a; 

float angle1 = - a; 


translate( robotX, robotY ); 
rotate(angle3) ; 

line(O, 0, 150, 0); 
translate(150, 0); 
rotate(anglez2) ; 

line(0, 0, 150, 0); 
translate(150, 0); 
rotate(anglet) ; 

line(O, 0, 150, 0); 
translate(150, 0); 


popMatrix(); 
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Your robot’s center pivot point is not 0,0 anymore. It’s been replaced by robotX and robotY. This is 
good because you can change it as you recalibrate/retune the robot. 
This is quite close to you want to achieve. You already have the following: 


* XY coordinates of the finger (pointer) 
¢ All three angles for the servos 
* Both coordinate systems tuned together 


By this point, you should see the virtual model of the robot in your work area, as shown in Figure 8-27. 


fo) robot table? 


Figure 8-27. Virtual robot driven by the user’s finger 


Polishing the Input 
Is this everything you need to drive the robot? Well, yes, but at this stage there is small danger arising. 


Looking at the behavior of the virtual instance of the robot, there’s something quite quirky about it. It’s 
shaky and performs unstable movements. 
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Due to the filtering of the point cloud and the way the lowest point above the table is captured, in 
some Cases you can observe very rapid movements of the arm. For example, when you point your finger 
to the middle of the working area, the robot will move there. But when you retract your hand from it, it 
will follow it at all costs—and will then suddenly get repositioned somewhere near the boundary 
(following the path of your hand). Sometimes the robot is somewhere on the table, and once you insert 
your hand into the scanning area, it will move towards it very quickly. 

This is okay in your virtual world. In the context of a physical installation, it could be dangerous. It 
might damage the robot, which is simply not able to move 50cm in 100 milliseconds. To avoid this, you 
need to add the following functionalities: 


¢ Movement smoothing 
¢ Further point cloud filtering, cutting off all points above certain height 


The first task is to remember all pointer positions and average the last 20 of them. The second is 
quite easy: just add another condition to the point cloud filtering statement. 
Let’s start with second one. Replace the if() statement in main scanning loop with this code: 


if ((mewPoint.z > zCutOffDepth - 50) 8&& 
(newPoint.z < zCutOffDepth) && 
(newPoint.x < 400) && 

(newPoint.x > -400) && 
(newPoint.y < 300) && 
(newPoint.y > -300)) { 


As you Can see, you just added one more statement at the beginning. This, in conjunction with 
second statement, only allows a thin slice of points to pass the filter. Its thickness is exactly 50mm. In 
practice, this means that you can withdraw the hand in upper direction, and it won’t make the robot 
follow it. It ignores the hand once it’s at a safe height. 

The first task, however, is slightly more complex: you need to build an array with all the positions of 
the pointer stored. Declare it somewhere at the beginning of the program, outside of the setup() and 
draw() functions. 


PVector [] path = new PVector[100]; //you're declaring array of 100 points 
Then allocate memory for it at the end of setup() function. 

for(int i = 0; i < 100; i++){ 
path[i] = new PVector(); 


Once this is done, you need to store pointer X and Y values in them. This is done by adding the 
following lines after the main scanning loop, but before you draw the robot: 


path[frameCount % 100].x 
path[frameCount % 100].y 


pointer.x; 
pointer.y; 


frameCount is the number of frames rendered from the beginning. The modulo operator (%) makes 
sure this never goes out of bounds, as the array is only 100 records long. (The last position in the array is 
number 99, actually, as you’re counting from 0.) 
Next, inside the robot drawing routine (inside of the pushMatrix() block, and after translate() 
command), add the following code: 
for (int i = 0; i < 100; i++){ 
ellipse(path[i].x, path[i].y, 5, 5); 


This displays your path in the form of small dots, following the pointer’s trajectory. You can test the 
code now to see how the trajectory shows. Make sure to wave your hand above the table; you can also 
use a Stick or a paper tube. 
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Finally, you need to average some values. Do this by taking the last 20 positions (you can use more 
or less, depending on robustness of the installation). So, instead of these two lines 


float penx 
float penY 


pointer.x; //you're capturing coordinates of your pen tip (mouse) 
pointer.y; 


write these two lines 


float penX = 0; //you're resetting pen coordinates 
float penY = 0; 
//and adding fractions of 20 last positions to them 
for (int i = 0; i < 20; i++) { 
penX += path[(frameCount + 100 - i) % 100].x/20.0; 
penY += path[(frameCount + 100 - i) % 100].y/20.0; 


Test the changes and observe how smooth the movement is now (Figure 8-28). This definitely 
makes it less prone to damage. 


‘B robot tabled sees _X 


SN 


Figure 8-28. User input smoothed 
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The Drawing Robot in Action 


The last step is to add a few extra instructions to send the robot angles to the Arduino and thus drive the 
robot. First, declare the libraries and the Arduino instance and then initialize it. Next, declare the 
variables defining the servo pins. The following block goes at the very beginning of the program: 


import processing.serial.*; 
import cc.arduino.*; 
Arduino arduino; 


int servoipin = 11; 
int servo2pin = 10; 
int servo3pin = 9; 


Within setup(), initialize the Arduino object and set the pin modes, after the size() command. 


arduino = new Arduino(this, Arduino.list()[1]); 
arduino.pinMode(servoipin, Arduino.OUTPUT) ; 
arduino.pinMode(servo2pin, Arduino.OUTPUT) ; 
arduino.pinMode(servo3pin, Arduino.OUTPUT) ; 


At the very end of the draw() function, add three lines of code that send the angles of the robot to the 
Arduino board using the Arduino library. Remember the reasoning behind the angle transformations 
from the “Driving the Physical Robot” section. 


arduino.analogWrite(servoipin, 90-round(degrees(angle1*1.0))); // move servo 1 
arduino.analogWrite(servo2pin, 90-round(degrees(angle2*1.0))); // move servo 2 
arduino.analogWrite(servo3pin, round(degrees(angle3))); // move servo 3 


And voila! After some initial play (you might still recalibrate it a bit), you can add a pen (by attaching 
it with a paper clip) and start sketching! Your small gestural control of servo-powered mechanism is 
finally working! 

Figures 8-29 and 8-30 show the robot at work. You can see a video of the piece at 
www. vimeo.com/34672180. 
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Figure 8-29. The drawing robot at work 


Figure 8-30. The robot drawing patterns on a sheet of paper 
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Summary 


This project had you building your own robotic drawing arm and programming it to follow the 
movements of your hand on a tangible table. You figured out how to parse the Kinect point cloud data 
using geometrical rules and how to acquire your finger position, which you used as the guide for the 
movements of the robotic arm. 

Unlike the rest of the projects in this book, this project didn’t require you to write any Arduino code 
at all. Instead, you learned how to use the Arduino Processing library and the Firmata Arduino firmware 
to control your motors directly from Processing. 

Further alterations and improvements on this project could include adding an external power 
supply for better responsiveness of the servos or adding a microservo to lift and lower the pen to control 
the drawing flow. 

I recommend spending some time calibrating this robot properly for a good match between the 
coordinate readouts from the Kinect’s scanning and the positioning of the arm. For advanced 
experimentators, you can try to hang the projector above the table and project an image of the virtual 
robot onto the physical one. It leads to proper calibration and adds a new layer of augmented 
information. 


CHAPTER 9 


Kinect Remote-Controlled Vehicles 


by Ciriaco Castro 


In Chapter 5, you learned how to hack a remote control in order to control different devices via your 
body movements. In this chapter, you are going a step further in the control of devices by hacking an RC 
vehicle’s electronics. You’re going to use a RC car, but the same logic can be applied to any RC vehicle 
(helicopter, boat, etc.). 

You are going to learn how to use an H-Bridge to control motors. Using the Kinect, you are going to 
produce an easy interface that connects body movements with the vehicle’s movements (Figure 9-1) and 
you'll apply what you learned from the XBee technology to do it wirelessly. Finally, you are going to 
create an automated routine that will avoid crashes by using a proximity sensor and certain 
programmed behaviors. Figure 9-2 shows the parts you'll need for this chapter and Table 9-1 lists the 
specifics. 


Figure 9-1. Kinect and the RC vehicle 
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Figure 9-2. RC vehicle components 


Table 9-1. The Parts List 


Part Description Price 

1 RC vehicle Pick a RC car big enough to carry an Arduino board Our cost 
(approximately 20 x 10 cm). Make sure ithastwo motors. $23.99 

2 XBee ImW Chip Antenna - XBee 1mW Chip Antenna - Series 1 $22.95 each 

Series 1 

XBee Explorer USB You will need a mini USB cable to connect it. $24.95 

XBee Explorer Regulated To interface the XBee with the Arduino $9.95 

1 prototype shield SparkFun DEV-07914 or similar $16.95 

23 Breakaway headers- You should have a bunch of these already from previous $1.50 

straight chapters. 

1 infrared distance sensor From SparkFun or similar $8.75 

1 motor driver Dual H-Bridge from SparkFun or similar $5.89 


Electrical Motors and the H-Bridge 


Before hacking and programming the car, let’s talk about electrical motors and how you are going to 
drive them using an H-Bridge. 
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An electric motor converts electrical energy into mechanical energy. Most electric motors operate 
through the interaction of magnetic fields and current conductors to generate the force. The kind of 
motor in most toys is a DC motor (Figure 9-3). A DC motor is an electric motor that runs on direct 
current (DC) electricity, usually from batteries. 


Figure 9-3. RC motor and H-Bridge 


To control a motor, you can turn it on and off using only one switch (or transistor). But controlling 
the direction is a bit more difficult. It requires a minimum of four switches (or transistors) arranged in a 
clever way. This can be simplified using an H-Bridge. 

H-Bridges are four switches (or transistors) arranged in a shape that resembles an H. Each H-Bridge 
can control two motors. Each H-Bridge has 16 pins (eight per side), as shown in Figure 9-4. Figure 9-5 
shows the schematic of an H-Bridge. 


ENABLE | Vcc 
IN 01 INO4 
OUT 01 OUT 04 
GROUND | GROUND 
GROUND | GROUND 
OUT 02 OUT 03 
IN 02 INO3 

Vc (SUPPLY) | ENABLE 02 


Figure 9-4. H-Bridge pins 
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+5V OUTO1 OUT 03 +5V 


04,05,12,13 


OUT02 OUT 04 
GROUND 


Figure 9-5. H-Bridge schematic 


You are going to use the H-Bridge to drive two motors. Each pair of channels is equipped with an 
ENABLE input (pins 1 and 9). A separate supply input is provided in pins 8 and 16, allowing operation at 
a lower voltage. Pins 4, 5, 12, and 13 are connected to ground. Each side of the H-Bridge has two 
transistors (pins 2 and 7, and pins 10 and 15). One is responsible for pushing this side to HIGH and the 
other for pulling this side to LOW. When one side is pulled HIGH and the other LOW, the motor spins in 
one direction. If you want the motor to spin in the opposite way, you just have to reverse the transistors 
(the first side LOW and the latter HIGH). 

Pins 3 and 6 are connected to the poles of one motor, and pins 11 and 14 are connected to the poles 
of a second motor. According to the direction of the current, the motor will spin in one direction or the 
opposite one. 


Hacking a Car 


Now that you understand these basic concepts of the motor, you can start to build your RC vehicle. The 
first step is to open up and observe what’s inside (Figure 9-6). You should see a circuit and a series of 
cables. Looking at the circuit, you can see a RC receiver that controls the behavior of the motors. If you 
follow the cables, there are two that connect the circuit to the batteries. Another two connect the circuit 
to the front motor, and another two connect the circuit to the back motor. RC cars normally have two 
motors. The front one turns the vehicle left and right. The back one provides traction, which is the 
moving of the vehicle frontwards and backwards. 
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Figure 9-6. RC car opened 


You're not going to use the existing circuit because you're going to build your own. So cut the cables 
that connect the actual circuit to the motors and the cables that connect to the batteries. Unscrew the 
board and leave the car ready for your new circuit (Figure 9-7). 


Figure 9-7. RC car, just motors and electrical cables 


You're going to drive the car using your Arduino. As a first step, you will be using serial data coming 
from a simple Processing sketch. Later, once everything is up and running, you will use the Kinect anda 
XBee module to drive the car with your body movements. 

Start by joining the power and ground cables coming from the battery pack to a power jack so you 
can plug your Arduino using just the external batteries (Figure 9-8). 
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Figure 9-8. RC car with the electrical connection to battery pack 


So far, you’ve had an overview of electrical motors and the H-Bridge. You have opened an RC 
vehicle, looked inside, and extracted the circuit in order to start to building a new one. 


Building the Circuit 


The next step is to prepare your Arduino board and connect it to an H-Bridge in order to control the 
two motors (the back one that provides the traction and the front one that provides the direction). In this 
project, you’re going to use a prototype shield instead of fusing the strip panel. A prototype shield is a 
platform for building a new project; it makes it easier to connect an Arduino to all the external bits and 
pieces. Shields are normally designed to sit on top of an Arduino, facilitating the connections. There are 
some pins that connect to the Arduino pins and there’s a prototyping area where you solder your 
external components. 

Start by soldering your breakaway headers in order to plug them into your Arduino board. Then 
solder two cables to 5V Arduino and ground and plug your H Bridge (Figure 9-9). 
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Figure 9-9. Prototype board and the H-Bridge 


Next, solder the H-Bridge and start making all the connections. The H-Bridge chip is going to be 
used as two bridges controlling two separate motors. Connect the H-Bridge’s pins 8 and 16 to 5V voltage. 
You can power the motors and the H Bridge using any external power device (connecting it to these 
pins), but remember to share the ground with the Arduino board. In this case, everything is powered 
through the Arduino (Figure 9-10). 


Figure 9-10. H-Bridge connected with power 


Then connect the H-Bridge pins 4, 5, 12, and 13 to ground. The next set of cables that you are going 
to solder are the transistors to Arduino pins. Link Arduino pins 11 and 5 with H-Bridge pins 10 and 15, 
and Arduino pins 6 and 10 with H-Bridge pins 2 and 7. The circuit will look like the one in Figures 9-11 
and 9-12. 
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Figure 9-12. H Bridge with inputs for power and ground (back) 


The next step is to connect the ENABLE inputs. Use a cable that connects H-Bridge pins 1 and 9 with 
Arduino pins 9 and 8. Remember to have a line in your code that allows them to be HIGH in order to 
activate the H-Bridge. The last step is to solder the H-Bridge OUT pins (pins 3, 6, 11, and 14) to the motor 
poles. Figures 9-13 and 9-14 show the circuit from the front and the back. 
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Figure 9-13. H-Bridge ready for connecting motors 
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Figure 9-14. H-Bridge ready for connecting motors (back) 


The diagram in Figure 9-15 shows the whole circuit. 
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BACK MOTOR FRONT MOTOR 


Figure 9-15. Circuit diagram 


Testing the Circuit 


It’s time to test the circuit. You are going to use a simple Processing sketch that will activate each motor 
according to the position of the mouse, using serial communication with Arduino (Figure 9-16). 
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| _1_Mouse_Control_Rev02 


else { 
Val62=8; 
; speed=B; 


if (tempO4>108 && temp@4+<150) { 
Val83~u148; 
speed= speed+6.5; 


} 

else if (temp4>158) { 
Val83=200 ; 
speed++; 


is tf (temp64<-108 8& temp84>-150) { 
Val83=140; 


speed= speed-8.5; 


= tf (temp04<-150){ 
Val83=200; 
speed-— 5 


} 
printin(val@t +" " + Val@2" "+ Vale); 


ellipse(width/2, height/2, width/3, width/3); we 
ellipse(vidth/2, height/2, width/6, width/6); : 
’ 


Figure 9-16. Processing sketch for testing 


The code imports the Serial library in order to create serial communication with Arduino. After that, 
a series of variables are defined in order to store the values that are going to be transmitted. Finally, 
other variables are used for building a little car simulation that shows a visual description of what are 
you transmitting. These variables are used inside a car() function. 


import processing.serial.*; 
Serial myPort; 
boolean serial = true; 


int Valo1, Valo2, Valo3; 
float tempo01, tempo2, temp03, tempo4; 


//car variables 

int carwidth= 70; 

int carheight =120; 

int wheels=30; 

float angle; 

float speed, wLine, wLinePos; 
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Setup Function 


The setup() function just declares the size of your sketch and defines the port used for transmitting the 
data. 


void setup() { 
size(300, 300); 
smooth(); 


if (serial) { 
String portName = Serial.list()[o]; 
myPort = new Serial(this, portName, 9600); 
} 
} 


Draw Function 


The draw() function defines the arrow from a series of PVectors. One is defined from the mouse position 
(end of the arrow) and the other from the center of the canvas. Once you know these positions, you 
create another PVector that is the subtraction of the first two. After that, you call the function 
drawVector() that is defined by two PVectors (and will be explained later). 


void draw() { 
background(255); 
PVector mouse = new PVector(mouseX, mouseY); 
PVector center = new PVector(width/2, height/2); 
PVector v = PVector.sub(mouse, center); 


//Draw vector between 2 points 
drawVector(v, center); 
noFill(); 


The next part of the code defines the values sent via serial communication. You map the X 
coordinates in a range of values from 0 (left) to 255 (right). In order to do this, you define a temporary 
value as the difference between the mouse’s X position and the canvas’ center. This value is remapped in 
the range in a temporary float. Then you introduce a series of if conditionals so the final value that you 
to transmit is 0 (no movement), 1 (turn left), and 2 (turn right). These values activate an action in your 
virtual car simulation. 

For the Y coordinate you also use a temporary value that refers to the mouse’s Y coordinate and the 
center of the canvas. You remap this value between -255 (bottom area), 0 (center), and 255 (top area). 
This is the value that you pass. 

The back motor is going to have two values: one that marks the direction (Val02) and other that 
marks the speed (Val03). The Val02 variable respond to 0 (don’t activate the motor), 1 (activate it 
forward), and 2 (activate it backward). So according to the position of the mouse, it will change; if it’s 
closer to the center, the motor won't be activated; if it’s far, the car will go forward or backwards. 

Another condition that you want to introduce is speed; this value depends on the distance of the 
mouse to the center. At the moment, just two speeds are defined: one of 140 and another of 200. 


tempO1 = mouseX-width/2; 
tempo2 = height/2- mouseY; 
tempo3= int(map(tempo1, -width/2, width/2, 0, 255)); 


//turn left 
if (tempo3<100) { 
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Valo1=1; 
angle=-PI/4; 


//turn right 
else if (tempo3>150) { 
Valo1=2; 
angle=PI/4; 


//no turn 
else { 
Valo1=0; 
angle=0; 


} 


// decide where to move 
tempo4= int(map(temp02, -height/2, height/2, -255, 250)); 
//move front 
if (tempo4>0 && tempo4>50) { 
Valo2=1; 


//move back 
else if (tempo4<o && tempo4<-50) { 
Valo2=2; 
} 


//don't move 
else { 

Valo2=0; 

speed=0; 


//decide speed 
if (temp04>100 && tempO4<150) { 
Val03=140; 
speed= speed+0.5; 


else if (tempo04>150) { 
Val03=200; 
speed++; 


else if (temp04<-100 && temp04>-150) { 
Val03=140; 
speed= speed-0.5; 


else if (tempo4<-150){ 


Val03=200; 
speed--; 
} 


The next lines of code just draw a couple of circles to define the motor area where the motor won’t 
work. This area is introduced in case you want to stop the car and not keep it constantly in movement. 
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Then you call the car() function that represents a virtual car on the screen. The last line of the draw() 
function just sends serial data. 


ellipse(width/2, height/2, width/3, width/3); 
ellipse(width/2, height/2, width/6, width/6); 


car(); 
if (serial){ 
sendSerialData(); 
} 


} 


DrawVector Function 


The drawVector() function represents an arrow starting from the center of your screen towards your 
mouse position (an arrow between two points). You mark the points using PVectors, but you also want 
to represent an arrowhead that points toward the direction that the car will move. 

The arrowhead is represented by two lines. But you have to do a bit of math in order to have the 
rotation. You translate locally the coordinate system using pushmatrix and translate. These functions 
locate a new local origin to be the reference for the rotation of the arrowhead. 

The angle that you rotate your coordinate system is defined by the PVector function heading2d. This 
function returns a float that defines the angle of rotation for the vector. So once your coordinate system 
has been rotated, you define the lines that form your arrow: a vertical line (remember that you have just 
rotate your UCS) that goes from 0,0 to the length (or magnitude) of your PVector, and for the head, you 
just define two lines from the extreme to both sides. 

The last step is to use popmatrix to close the translations and rotations. 


void drawVector(PVector v, PVector loc) { 
pushMatrix(); 
float arrowsize = 4; 
translate(loc.x, loc.y); 
stroke(0); 
rotate(v.heading2D()); 
float len = v.mag(); 
line(o, 0, len, 0); 
line(len, 0, len-arrowsize, +arrowsize/2); 
line(len, 0, len-arrowsize, -arrowsize/2); 
popMatrix(); 


Car Function 


The car() function represents a diagramatic verison of your car. The central rectangle defines the body 
and the four small ones represent the wheels. Select a blue color and use the functions pushMatrix and 
popMatrix to represent each rectangle on the screen, allowing rotation for the front wheels. So, 
according to the values received (or the mouse position), the wheels turn in one direction or the other. 
Finally, to represent the speed, you define a red line that crosses the back wheels, simulating the 
rotation of the wheels. 


void car() { 
rectMode(CENTER) ; 
//body 
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noFill(); 
stroke(30, 144, 255); 
rect(width/2, height/2, carwidth-wheels/2, carheight) ; 


//fxront wheels 

pushMatrix(); 

translate(width/2-carwidth/2+wheels/4, height/2-carheight/2+wheels) ; 
rotate(angle) ; 

rect(0, 0, wheels/2, wheels); 

popMatrix(); 

pushMatrix(); 

translate(width/2+carwidth/2-wheels/4, height/2-carheight/2+wheels) ; 
rotate(angle) ; 

rect(0, 0, wheels/2, wheels); 

popMatrix(); 


//back wheels 

pushMatrix(); 

translate(width/2, height/2); 

rect(-carwidth/2+wheels/4, carheight/4, wheels/2, wheels); 

rect(carwidth/2-wheels/4, carheight/4, wheels/2, wheels); 
//line simulating speed 

stroke(255, 0, 0); 

line(-carwidth/2, wline, -carwidth/2+wheels/2, wLine); 

line(carwidth/2, wline, carwidth/2-wheels/2, wLine); 

wLine=(carheight/4+wLinePos+speed) ; 


if (wline<carheight/4-wheels/2) { 
wLine=carheight/4+wheels/2; 
speed=0; 
wLinePos=wheels/2; 


} 

else if (wLine>carheight/4+wheels/2) { 
wLine=carheight/4-wheels/2; 
speed=0; 
wLinePos=-wheels/2; 


} 
popMatrix(); 


SendSerial Function 


The sendSerial() function is the same as in previous chapters; it just sends data with a letter before it in 
order to recognize and avoid any loss of data. 


void sendSerialData() { 
myPort.write('S'); 
myPort .write(Val01) ; 
myPort .write(Valo2) ; 
myPort .write(Valo3) ; 
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With this code, you send four values of serial data to drive your motors. The first value is just to 
identify the car, the second value is a range for moving the front motor, the third value defines a 
direction, and fourth value defines a speed. Now you have a sketch that sends serial data to your Arduino 
according to your mouse position. You are really close to your first test but first you need to program 
your Arduino. 


Arduino Testing Sketch 


The code starts by declaring floats and the Arduino pins that are connected to your H-Bridge. Note that 
for the back motor you use analog pins. 


float Valo1, Valo2, Valo3; 

int motorFrontLeft = 11; // H-bridge leg 10 
int motorFrontRight = 5; // H-bridge leg 15 

int motorBackUp = 6; // H-bridge leg 2 (PWM) 
int motorBackDown = 10; // H-bridge leg 7 (PWM) 
int enablePinFront = 8; // H-Bridge leg 9 

int enablePinBack = 9; // H-Bridge leg 1 


° 
d 
. 
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Setup Function 


The setup() function just declares the start of serial communication and the Arduino pin mode; in this 
sketch all pins are outputs. 


void setup() { 
// initialize the serial communication: 
Serial. begin(9600) ; 
// Set the pin modes 
pinMode(motorFrontLeft, OUTPUT) ; 
pinMode(motorFrontRight, OUTPUT) ; 
pinMode(motorBackUp, OUTPUT) ; 
pinMode(motorBackDown, OUTPUT) ; 
pinMode(enablePinFront, OUTPUT) ; 
pinMode(enablePinBack, OUTPUT) ; 


Loop Function 


Start by reading the serial data. If any data is available, the first piece of information should match the 
letter S. If this is the case, continue reading and store the values as floats. 

After reading the values, you introduce a series of if conditionals that will drive the motors 
according to the values obtained. For the front motor drive, you get values 1 (turn left), 2 (turn right), and 
0 (no action in case you want to drive in a straight line). If the values are 1 or 2, the H-Bridge’s ENABLE 
pin is active, meaning that the motor starts to work and the direction is defined by the functions 
TurnLeft() and TurnRight(). 

For the back motor, you receive two values (Valo2 and Val103); one defines the direction and the 
other defines the speed. 

Variable Val02 defines the direction. If its value is 1, it puts the H-Bridge ENABLE pin as HIGH and 
calls the MoveUp() function that is defined by a value (speed). If variable Val02 is 2, the ENABLE pin is 
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HIGH and calls the function MoveDown() runs, moving the car in the opposite direction. Finally, if variable 

Val02 is 0, the ENABLE pin is LOW, meaning that this motor is disconnected and the car won’t move. 
Variable Valo3 defines the speed of the motor. Potentially, you could have values from 0 to 255, but 

according to the Processing sketch you just have two speeds, normal and fast, with values of 140 and 200. 


void loop() { 
// check if data has been sent from the computer: 
if (Serial.available()>4) { // If data is available to read, 
char val = Serial.read(); 


if(val == 'S') { 
Valo1 = Serial.read(); 
Valo2 = Serial.read(); 
Valo3 = Serial.read(); 
} 


} 


// turning left and right 

if (Valoi==1) { 
enable(enablePinFront) ; 
TurnRight(); 


else if (Valo1==2) { 
enable(enablePinFront) ; 
TurnLeft(); 


else if(Val01==0)disable(enablePinFront) ; 
if (Valo2==1) { 
enable(enablePinBack) ; 
MoveUp(Val03) ; 


else if (Valo2==2) { 
enable(enablePinBack) ; 
MoveDown(Val03) ; 


else if(Valo2==0) { 
disable(enablePinBack) ; 


} 


Turning Functions 


The functions for turning right and turning left are similar. Each function sets one pin to HIGH and one 
to LOW. The direction of the current flow, and therefore the direction of the motor, is determined by 
which pin is in which state. The function is straightforward: a digitalWrite is applied to the Arduino 
pins. 


//function to turn Right 

void TurnRight(){ 
digitalWrite(motorFrontRight ,HIGH) ; 
digitalWrite(motorFrontLeft, LOW) ; 
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} 


//function to turn Left 

void TurnLeft(){ 
digitalWrite(motorFrontLeft,HIGH) ; 
digitalWrite(motorFrontRight, LOW) ; 


} 


Move Functions 


The move functions are similar to the turning functions; the main difference is that you use analogWrite 
with an external value defined in the void. This value marks the speed of rotation of the motor. So instead 
of using HIGH and LOW, you use speed: 255 as a maximum value and 0 to indicate the car has stopped. 


//function to move 

void MoveUp(int speedD) { 
analogWrite(motorBackUp, speedD) ; 
analogWrite(motorBackDown, 0) ; 

} 

//function to move reverse 

void MoveDown(int speedD){ 
analogWrite(motorBackDown, speedD) ; 
analogWrite(motorBackUp,0) ; 


} 


Enable/Disable Functions 


The enable and disable functions just write HIGH or LOW to the pin marked when they are called. You 
use them for the H-Bridge ENABLE pins, activating or disconnecting the motors. 


void enable(int pin) { 
digitalWrite(pin, HIGH); 


void disable(int pin) { 
digitalWrite(pin, LOW); 


Now you're ready to test your car for the first time! Upload your Arduino code (leaving the USB 
cable plugged into the car; you need it for the serial communication). Run the Processing sketch and 
move your mouse. The car should move and turn according to your mouse position! 


Proximity Sensors 


Now let’s introduce a bit more complexity. You are going to use a proximity sensor in the front part of 
the car to check the distance from another object. Using the values it returns, you are going to introduce 
a routine of “stopping the car” inside your Arduino code. The idea is to avoid collisions. The code will let 
you control the car but if it is going to crash with an object, it will override your movements and 
automatically stop (and actually move backwards a little). 
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The routine will check the value coming from the proximity sensor and it will compare it with a 
threshold (a value that we have defined previously in the code). 

If this value is higher than the threshold it will mean that an object is really close. If that is the case, 
the back motor will start to turn backwards for a short period of time and after that it will check again the 
value coming from the sensor, repeating this routine if the value is still bigger than the threshold. 

If the value from the sensor is smaller than the threshold it means that there is not any object close 
enough, and there is no risk of crashing, so the car will move normally. 

An analog infrared (IR) distance sensor is a really simple component (Figure 9-17). It shines a beam 
of IR light from an LED and measures the intensity of light that is bounced back using a phototransistor. 
If you film this sensor with a cellphone or a camera and you look at the sensor through the screen, you 
will see the IR LEDs glowing. 


Figure 9-17. Infrared proximity sensor 


To make it work, just connect +5v and ground, and an analog signal will be returned (Figure 9-18). 
This signal is in a voltage proportional to the distance between the sensor and an object in front of it. 
The sensor you are using has a range of 80 cm (0.4 volts) and 10 cm (3 volts). 
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Figure 9-18. Proximity sensor connected to Arduino 
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You can work with the values that come from the sensor if you choose an appropriate maximum 
value to act as threshold in order to activate your stop car function. 

The IR proximity sensor has been used in many interactive applications. Even if you are just using 
the outcome voltage coming from the sensor, it’s worthwhile to know how the sensor works and how 
you can measure real distances with it. (You can skip this explanation if you want.) 

The voltage returned is not linear, as you can see in Figure 9-19. 
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Figure 9-19. Proximity sensor’s voltage/distance graph 


However, you can convert the voltage returned to distance using an equation. Depending of the model of 
sensor, this equation will vary. For the Arduino code, the equation for transforming inputs to distance is 


float distance = 12343.85 * pow(analogRead(sensorPin) ,-1.15); 


Before adding it to your circuit, do a quick test by connecting a sensor to your Arduino board using a 
pigtail or an infrared sensor jumper. Use this code for the test: 


int sensorPin = 0; 
void setup(){ 
Serial. begin(9600) ; 


void loop(){ 
int val = analogRead(sensorPin) ; 
//float val = 12343.85 * pow(analogRead(sensorPin) ,-1.15); 
Serial. print1n(val) ; 
delay(100) ; 


Use Arduino analog Pin 0 for receiving the values, and start the serial communication. Next, choose 
which values you want out of the Arduino: the voltage (int val) or the distance (float val). Finally, just 
add a delay to slow down the output. 
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ProximitySensor_test | Arduino 0022 


SensorPin = @; //anaiog pin o 


Did setup(}t 
Ser tal .begin(9688); 


Did toop()f] 
int val « anologReod(sensorPin); 
//f loot val = 12343.85 * pow(analogRead(sensorPin),-2.15); 


Serial .printin(val); 


//just to slow dow the output - remove if trying to catch on object possing by 


Gelay(108); 
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Figure 9-20. Proximity sensor’s output values 


The values that you obtain in the Serial Monitor, depending on the code you use, are between 650 
(really close) and 20 (object far away), as shown in Figure 9-20. If we comment the int val and 
uncomment the line float val, we will use the equation previously described in the code. If we upload 
this “modified” code to Arduino we will obtain values between 70 and 7, meaning real distance in 
centimeters. Using this equation is great because we have real values, but it also gives us a big error 
when the object is really far away, due to the math that you applied.) 

Enough theory! Let’s get back to the RC vehicle. To keep things simple, use the voltage values. In 
your case, just knowing the threshold value that indicates that your car is close to an object is enough to 
activate your stop car routine. 

After testing, add the sensor to your car circuit. Just solder the cables to power, ground, and analog 
pin 0 in your prototype board. The circuit looks like the one in Figure 9-21. 


227 


CHAPTER 9} KINECT REMOTE-CONTROLLED VEHICLES 


228 


Figure 9-21. Proximity sensor connected 


Fix the sensor on the front area of the car using some removable adhesive (in case you want to use 
the sensor for another application), as shown in Figure 9-22. 
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Figure 9-22. Proximity sensor on car’s front 


Now change the Arduino code in order to get an automated response according to the data coming 
from the sensor. As mentioned, the idea is to stop the car and reverse it for long enough to avoid a 
collision. If the data coming from the sensor is high (meaning an object is really close), the back motor 
will automatically reverse. 

Add a few variables to the beginning of the code. A new variable (Val04) activates or deactivates the 
automatic response. The sensor is incorporated with an analog IN pin (pin0O), an initial value, anda 
threshold value to mark the distance from which the automated response will be active. 


float Valo1, Valo2, Valo3, Valo4; 
(ess<) 


int sensorPin = 0; 
int sensorValue=0; 


int Value =500; //threshold for detecting object 


Setup Function 


The setup() function remains the same. 


Loop Function 
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The loop() function incorporates this new value from the serial data. According to the value of Valo4, 
Arduino reads the data coming from the sensor (automatic response on) or just marks this value as 0 


(automatic response off). 


Then you add a general if before all the functions, marking the value coming from the sensor and 
comparing it to the threshold. In other words, if no object is close, you drive the vehicle with your 
Processing sketch, but if an object is nearby, the code jumps directly to the new function called 


stopCar(). 


void loop() { 
if (Serial.available()>5) { 
char val = Serial.read(); 


if(val == 'S'){ 
Valo1 = Serial.read(); 
Valo2 = Serial.read(); 
Valo3 = Serial.read(); 
Valo4 = Serial.read(); 
} 


} 


if (Valoq= 1)sensorValue = analogRead(sensorPin) ; 


else sensorValue=0; 


// turning left and right 
if (sensorValue<Value) { 
if (Valoi==1){ 
enable(enablePinFront) ; 
TurnRight(); 


else if (Valo1==2) { 
enable(enablePinFront) ; 
TurnLeft(); 


else if(Val01==0)disable(enablePinFront) ; 
if (Valo2z==1) { 

enable(enablePinBack) ; 

MoveUp(Val03) ; 
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else if (Valo2z==2) { 
enable(enablePinBack) ; 
MoveDown(Val03) ; 


else if(Valo2==0) { 
disable(enablePinBack) ; 
} 
} 


else { 
enable(enablePinBack) ; 
stopCar(); 


Move Functions 


All move functions remain the same. You just add a new function stopCar() that exists only to call the 
previous function MoveDown() and disable the back motor once it has moved backwards. This function 
acts as a brake, avoiding collisions. 


void stopCar(){ 
for(int i=0; i<2000; i++){ 
MoveDown(200) ; 


disable(enablePinBack) ; 


Once you have connected the proximity sensor, you can test your car. Upload this new Arduino 
code (you are still not wireless, so you need to leave the USB cable plugged to your Arduino board). Run 
the previous Processing sketch again and move your mouse. The car should move as before, but if you 
put an object in front of it, it should stop and then move backwards. 


XBee and Wireless 


The last step is to add a wireless module. You learned how to use XBee modules for wireless 
communication in pervious chapters. (Have a look at those chapters for a refresher on the concepts and 
how to install drivers in case you haven’t done it before.) 

As you have seen, you can use an XBee shield or just connect the cables that you need. In this case, 
you want to everything as compact as possible, so solder the cables coming from the XBee Explorer as 
described in Table 9-2 and shown in Figure 9-23. 


Table 9-2. XBee Explorer Regulated to Arduino Connections 


XBee Explorer Regulated Pins Arduino Pins 
XBee 3.3V Arduino 3.3V 
XBee GND Arduino GND 
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XBee Explorer Regulated Pins Arduino Pins 

XBee DIN Arduino TX (Digital Pin 1) 
XBee DOUT Arduino RX (Digital Pin 0) 
XBee DIO3 Arduino RESET 


Figure 9-23. XBee Explorer 


If you solder the cables on your prototype shield, the circuit will look like that in Figure 9-24. 


Figure 9-24. XBee on the circuit 


Now just tidy up the cables and fix them to the vehicle to avoid disconnections when the vehicle is 
in movement. The final vehicle will look like Figure 9-25. 
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Figure 9-25. The complete hacked RC car 


The very last step is to prepare a Kinect sketch and start driving your car. But first, test the car. Plug 
your XBee Explorer USB to your computer, run the Processing sketch, and make sure that data is getting 
to your Arduino. You should see a flashing Arduino LED Rx. If that’s happening, you should be able to 
drive your car just using your mouse. 


Kinect RC Interface 


Now you're going to use the movement of your hand to drive the vehicle. 


According to its relative position to the center of the screen, you can drive the car, moving it 
towards this direction. 


If your hand is in the central position, the vehicle won’t move. 


If you draw a circle with your hand, you activate a virtual switch, turning on and off the 
automatic response, as shown in Figure 9-26. 


The automatic response marks if Arduino is going to read the data coming form the sensor, and 
therefore it will react automatically when an object is really close using the function stopcar(). 


In the case this automatic response is off, the data coming from the sensor will be overwrite to 
0, meaning that the car will be “blind” respect the objects that are in front and won’t have any 
automatic reaction. 


Note that this circle should be drawn close to the center of the screen where the car is not in 
movement so the gesture of doing a circle won’t drive the car over the place. 
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Figure 9-26. Safety mode on and off 


Similar code has been used in the previous chapters. You’re just creating a few new functions and 
putting some text on the screen in order to display live data. The code is based on hand and circle 
recognition. Start by importing the libraries. 


import SimpleOpenNI.*; 
import processing.opengl.*; 
import processing.serial.*; 


SimpleOpenNI kinect; 
Serial myPort; 


Next, declare the NITE objects explicitly, using the NITE functions for hand tracking, gesture 
recognition, and circle detection. You declare the session manager (that deal with NITE gesture 
recognition), control points (that deal with hand points), and circle detector. 


// NITE 

XnVSessionManager sessionManager; 
XnVPointControl pointControl; 
XnVCircleDetector circleDetector; 


You're going to work with text on the screen so you declare a font and some strings. (You declare 
them here in order to avoid null values once you start your Processing sketch. They update their value 
once the hand has been detected.) 


// Font for text on screen 
PFont font; 

String Ctext; 

String dir; 
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The other variable that you declare is handTrackFlag, which refers to a hand being detected. The 
same principle is applied to the boolean circleTrackFlag. Other variables that you declare are PVectors 
that represent the positions of the hand on the screen, and the position of the hand in the real world. 
Then other PVectors will represent the center of a circle when we do the gesture and again we use two 
different variables: one representing the center on the screen and another representing the center in the 
real world. Other variables are radius and a counter t, that will serve for changing the state of a Boolean 
activating or deactivating the automated response. PVector v is the difference between the screen center 
and your hand position but in 2D. Boolean automated sends data about using the proximity sensor or 
not, and Boolean serial allows you to send serial data. The last set of values is a set of temporary ones 
that are remapped before being sent via serial communication to Arduino. 


// Nariables for Hand Detection 

boolean handsTrackFlag = false; 

boolean circleTrackFlag = false; 

PVector screenHandVec = new PVector(); 
PVector handVec = new PVector(); 

int t = 0; 

float rad; 

PVector centerVec = new PVector(); 
PVector screenCenterVec = new PVector(); 


PVector v = new PVector(); 
boolean automated=true; 


boolean serial = true; 
int Valo1, Valo2, Valo3, Valo4; 
float tempo1, tempo2, tempo3, tempo4; 


Setup Function 


In the setup() function, you initialize all the objects that you will be using later on, starting from the 
Kinect ones and then calling the NITE session manager where listeners will be added. As explained in 
previous chapters, you need to call methods create, destroy and update, so the methods XnVControl() 
and XnVDetector() invoke your callback functions in order to create, destroy and update one object (a 
circle or a hand detection). 


void setup() { 

// Simple-openni object 

kinect = new SimpleOpenNI(this) ; 
kinect.setMirror(true) ; 

// enable depthMap generation, hands + gestures 
kinect.enableDepth(); 

kinect.enableGesture(); 

kinect.enableHands(); 


// setup NITE 

sessionManager = kinect.createSessionManager("Wave", "RaiseHand"); 
// Setup NITE.s Hand Point Control 

pointControl = new XnVPointControl(); 
pointControl.RegisterPointCreate(this) ; 
pointControl.RegisterPointDestroy(this) ; 
pointControl.RegisterPointUpdate(this) ; 
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// Setup NITE's Circle Detector 
circleDetector = new XnVCircleDetector(); 
circleDetector.RegisterCircle(this) ; 
circleDetector.RegisterNoCircle(this) ; 


// Add it to the session 
sessionManager .AddListener(pointControl) ; 
sessionManager .AddListener(circleDetector) ; 


After calling the Nite methods, add the size of your canvas and initialize the font (which should be 
added to the data folder). You also initialize the Strings that you defined before and finally start your 
serial communication (depending on a Boolean, so it’s easy to connect and disconnect). 


// Set the sketch size to match the depth map 
size(kinect.depthWidth(), kinect.depthHeight()); 
smooth(); 


// Initialize Font 
font = loadFont("SansSerif-12.vlw"); 
Ctext="Automated mode ON"; 
dir = "-"; 
//Initialize Serial Communication 
if (serial) { 
String portName = Serial.list()[0]; // This gets the first port 
myPort = new Serial(this, portName, 9600); 
i 
} 


Draw Function 


The purpose of this sketch is to send data to Arduino, according to the hand position and the gesture 
that you perform in front of the Kinect camera. You start by calling a black background and a PVector to 
mark the center of the screen, as shown in Figure 9-27. 


Direction: $ 


Speed: 140 f 
Aukomated mode ON = Automated mode ON 


Figure 9-27. Driving the car 
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After that, you update the Kinect object so you can see what your Kinect is seeing. 


void draw() 


background(0) ; 

PVector centerL = new PVector(width/2, height/2); 
// Update Kinect data 

kinect.update(); 

// update NITE 

kinect.update(sessionManager) ; 


// draw depthImageMap 
image(kinect.depthImage(), 0, 0); 


Next, you calculate the difference in 2D (just XY coordinates) between the projection of the hand 
and the center of the canvas. This operation should be familiar; remember your Processing test sketch? 
You are doing the same thing but using the data coming from the Kinect. 


//displacement between the centre and the hand in 2D 
v.X = screenHandVec.x-centerL.x; 
v.y = screenHandVec.y-centerL.y; 


Next, call a series of functions depending on whether the Kinect is tracking a hand. If so, it draws a 
hand and an arrow and then drives the car. If the Kinect detects a circle, it draws on the screen. A series 
of functions is running and updated continuously: some text on the canvas and the data that you are 
sending via serial. 


if (handsTrackFlag) { 
drawHand(); 
drawArrow(v, centerL); 
controlCar(); 


if (circleTrackFlag){ 
drawCircle(); 


textDisplay(); 
if (serial){ 
sendSerialData(); 


} 


Other Functions 


The function controlCar() maps all the data in just a few values in order to transmit them to the 
Arduino. This function is similar to the one used previously in the Processing sketch. It just remaps the 
arrow vector according to the coordinates X and Y and then stores these values. It uses temporary values 
to remap them with a series of if conditions in order to have a clean output. 


void controlCar() { 


tempO1 = screenHandVec.x-width/2; 
tempo2 = height/2- screenHandVec.y; 
tempo3 = int(map(tempO1, -width/2, width/2, 0, 255)); 


if (tempo3 < 75) { 
Valo1 = 1; 
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if (tempo3 > 175) { 
Valo1 = 2; 


} 
if ((tempo3 > 75) && (tempo3 < 175)) { 
Valo1 = 0; 


tempo4= int(map(temp02, -height/2, height/2, -255, 250)); 
if ((tempo4 > 0) && (tempo4 > 50)) { 


Valo2 = 1; 

} 

else if ((tempo4 < 0) && (tempo4 < -50)) { 
Valo2 = 2; 

else { 
Valo2 = 0; 


} 
if ((tempo4 > 100) && (tempo4 < 150)) { 
Valo3 = 140; 


} 
if (tempo4 > 150) { 
Valo3 = 200; 


if ((tempo4 < -100) && (tempo4 > -150)) { 
Valo3 = 140; 


} 
if (tempo4 < -150) { 
Val03 = 200; 


//printin(Valo1 +" " + Valoz+ "  " + Valo3+ "  "  +4Valo4 ); 


The Other function represents the hand on the screen. Once the hand is detected, it appears as a red 
dot that makes the same movements as your hand. There’s also a function for representing a circle in 
case the gesture is detected. With the counter t, you can change the Boolean automated every time the 
new circle is produced. So the automated behavior is activated and deactivated with the same function. 
If you draw a circle, the automated behavior is turned off, and if you repeat the gesture, it’s on again. 
These gestures also change the color of the circle and the text referring to the automated mode as well as 
Valo4, which is sent via serial. 


// Draw the hand on screen 
void drawHand() { 


stroke(255, 0, 0); 

pushStyle(); 

strokeWeight (6) ; 
kinect.convertRealWorldToProjective(handVec, screenHandVec) ; 
point(screenHandVec.x, screenHandVec.y) ; 


popStyle(); 


void drawCircle() { 
if(t == 1)automated=! automated; 
if (automated == true){ 
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Valo4 = 0; 

noFill(); 

strokeWeight (6) ; 

stroke(0, 255, 0); 

ellipse(screenCenterVec.x, screenCenterVec.y, 2*rad, 2*rad); 
textAlign(LEFT) ; 

Ctext = "Automated mode ON"; 


} 
if (automated==false) { 
Valo4 = 1; 
noFill(); 
strokeWeight (6); 
stroke(255, 0, 0); 
ellipse(screenCenterVec.x, screenCenterVec.y, 2*rad, 2*rad); 
textAlign(LEFT) ; 
Ctext = "Automated mode OFF"; 


} 
// print1n(automated) ; 


The function drawArrow represents an arrow between two PVectors. It was used at the beginning of 
the chapter and it is pretty straightforward. 


void drawArrow(PVector v, PVector loc){ 
pushMatrix(); 
float arrowsize = 4; 
translate(loc.x, loc.y); 
stroke(255, 0, 0); 
strokeWeight (2); 
rotate(v.heading2D()); 
float len = v.mag(); 
line(o, 0, len, 0); 
line(len, 0, len-arrowsize, +arrowsize/2); 
line(len, 0, len-arrowsize, -arrowsize/2); 
popMatrix(); 


textDisplay represents live text (meaning that the values update automatically) on the screen. The 
function text asks for a value or string plus X and Y positions. In this sketch, you are showing the car’s 
direction, speed of the back motor, and whether the automated mode is activated or not. According to 
the values you are sending to Arduino, you make the rules for representing these values using a series of 
if conditionals. The last function is sendSerialData, which as usual uses a letter to identify each message. 


void textDisplay(){ 
text(Ctext, 10, kinect.depthHeight()-10); 
int value; 
if(Valo2 == 0) { 
value=0; 


} 
else { 
value = Valo3; 


} 
text("Speed: "+value, 10, kinect.depthHeight()-30); 
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text("Direction: "+dir, 10, kinect.depthHeight()-50); 


if ((Valo2 == 1) && (Valo1 == 0)) { 
dir ="N"; 


i, ((Valo2 == 2) && (Valo1 == 0)) { 
. ((Valo2 == 0) && (Valo1 == 1)) { 
i, ((Valo2 == 0) && (Valo1 == 2)) { 


if ((Valo2 == 1) && (Valo1 == 2)) { 
dir="NE"; 


fe ((Valo2 == 2) && (Valo1 == 2)) { 
dir="SE"; 


if ((Valo2 == 2) && (Valo1 == 1)) { 
dir="SW"; 


} 
if ((Valo2 == 1) && (Valo1 == 1)) { 
dir="NW"; 
} 
} 


void sendSerialData() { 
// Serial Communcation 
myPort .write('S'); 
myPort .write(Val01) ; 
myPort .write(Val0o2); 
myPort .write(Valo3) ; 
myPort .write(Val0o4) ; 


The last part is the NITE callbacks. As shown in previous chapters, they refer to the events of 
creating an object (hand or circle) once the gesture is detected, destroying it when the hand or the circle 
has gone off the screen, and updating them. Hand events and circle events have different callbacks. The 
only lines that are new are the ones that refer to the counter t. Each time a new circle is created, the 
counter starts to add, meaning that just once it will have the value of 1 (which is necessary for changing a 
Boolean). Each time that circle is destroyed, this counter comes back to 0. 


// XnVPointControl callbacks 
void onPointCreate(XnVHandPointContext pContext) { 
println("onPointCreate:"); 
handsTrackFlag = true; 
handVec.set(pContext.getPtPosition().getX(), 
pContext.getPtPosition().getY(), 
pContext.getPtPosition().getZ()); 
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void onPointDestroy(int nID) { 
println("PointDestroy: " + nID); 
handsTrackFlag = false; 

} 


void onPointUpdate(XnVHandPointContext pContext) { 
handVec.set(pContext.getPtPosition().getX(), 
pContext.getPtPosition().getY(), 
} pContext.getPtPosition().getZ()); 


// XnVCircleDetector callbacks 

void onCircle(float fTimes, boolean bConfident, XnVCircle circle) { 
println("onCircle: "+ fTimes + " , bConfident=" + bConfident) ; 
circleTrackFlag = true; 
t++; 
centerVec.set(circle.getPtCenter().getX(), 

circle.getPtCenter().getY(), handVec.z); 

kinect.convertRealWorldToProjective(centerVec, screenCenterVec) ; 
rad = circle.getFRadius(); 


} 


void onNoCircle(float fTimes, int reason) { 
println("onNoCircle: " + fTimes + " , reason= " + reason); 
circleTrackFlag = false; 
t=0; 


} 


Now, test this code with your vehicle. The Arduino code hasn’t changed, so just plug the Kinect and 
the XBee USB Explorer. Run the new Processing sketch and start to drive, as shown in Figure 9-28! 


Figure 9-28. Driving your RC vehicle 
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Summary 


If you use this code (make sure that the Boolean serial is set to true), you will be sending serial data to 
drive a hacked RC car. Using the position of your hand, you will be able to drive it; using a gesture, you 
can activate a simple anti-collision system. 

You have learned several new things in this chapter. Regarding hardware, you learned about RC 
motors and how to drive them using a simple H-Bridge. You opened a remote control car and, using the 
components, reprogrammed its behavior using Arduino. You also learned how proximity sensors work 
and the kind of data you can obtain from them. 

This application is only the principle; the same logic can be applied to any RC vehicle. Next time, 
hack a helicopter or a boat! Just open it up, investigate what’s inside, and start to have fun! 
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Biometric Station 


by Enrique Ramos 


In 1487, Leonardo da Vinci created a drawing that he entitled “The Vitruvian Man,” depicting what he 
believed to be the ideal human proportions. But these proportions are not only a matter of beauty. 
Height, weight, and other body measures can actually be an indicator of appropriate development and 
general health in both children and adults. Kinect has a device that can precisely measure all your body 
proportions in real time. If you combine this ability with other sensors, you can create a general-purpose 
biometric station for your personal use and delight. 

Throughout this chapter, you will learn how to hack a bathroom scale and connect it wirelessly to 
your computer, how to combine the incoming data with the Kinect point cloud and skeleton data, and 
how to compare this information to external databases for different purposes. Table 10-1 shows the 
parts required for this project. You will be able to identify people, automatically get body mass indexes, 
and check whether they fall within desirable ranges, as shown in Figure 10-1. You will store that 
information so you can create graphs of the evolution. 

This is an advanced chapter in terms of programming techniques, and it will introduce some Java 
file parsing techniques and the use of Java AWT components. Welcome to Kinect biometrics! 


Figure 10-1. A Kinect user being scanned and recognized 
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Table 10-1. Parts list 


Part Description Price 
Electronic bathroom scale With LCD display! $20-40 
XBee Shield To interface the XBee with your Arduino $24.95 
XBee Explorer USB You will need a mini USB cable to connect it $24.95 

2 XBee modules XBee 1mW Chip Antenna - Series 1 $22.95 each 
Stackable headers 6-pin and 4-pin. At least two of each $0.50 each 
1 Prototype Shield SparkFun DEV-07914 or similar. $16.95 


Hacking a Bathroom Scale 


In this project you will combine weight information with volume information from the Kinect sensor. 
You learned how to acquire Kinect data in a previous chapter, so now you need to figure out how to get 
the weight information into the computer. Well, one option is to type the information in, but that would 
be too easy, wouldn’t it? You’re going to go a little further and hack a standard bathroom scale to 
communicate wirelessly with your computer, so you can get the volume and weight information at the 
same time (Figure 10-2). 


Figure 10-2. Scale after removing the cover and LCD 
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Hacking a bathroom scale would seem to be a simple task. If you have previously worked with 
pressure sensors, you might think that it’s just a matter of finding the analog output, reading it from your 
Arduino, and mapping the values to the real weight. Well, it happens to be slightly more complicated 
than that. Most modern scales use the combined output of one to four strain gauges. The main PCB 
processes this information and displays it on the LCD. 

If, after a close inspection of your PCB, you don’t find any analog output that makes sense regarding 
weight data, look at the back of the PCB. There’s often a three-pin breakaway header (Figure 10-3) that 
provides an analog signal that changes depending on the state of the scale (off, weighing, weight found). 
You will see how to use this information later. 


Figure 10-3. PCB breakaway headers connected to an analog sensor cable 


Trying to get information from the strain gauges directly seems downright impossible, so why not 
let the PCB do its job and just sniff the weight information as it’s passed to the LCD? This lets the PCB do 
all computation and means that you get the same numbers as are shown on the LCD. 

LCD hacking is a very handy technique indeed; the same approach can be applied to any other 
appliance or device with a built-in LCD (which is pretty much every single device you can buy today). 
You might as well be getting the minutes remaining before you can eat your microwaved meal or the 
temperature in your home boiler. But for the moment, let’s stick to the weight on your bathroom scale. 


Seven-Segment LCD 


Most bathroom scales, and actually most built-in LCDs, are seven-segment displays (Figure 10-4). This 
includes calculators and all kind of home appliances. They are widely used because this is a very 
optimized way of displaying decimals compared to dot-matrix displays, as every number can be 
encoded in one byte of information; seven bits defining the number, and one extra for the decimal point. 
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Figure 10-4. Individual segments (from Wikipedia), and the LCD on your scale. 


The information is usually encoded in the order of gfedcba or abcdefg, but you can find other 
encodings, as you will see in this chapter. You have to check the signals from the PCB to the LCD to 
figure out the encoding of your scale, so first let’s hack the scale. 


Hacking the LCD 


Once you have removed the cover and exposed the guts of your scale, you’re probably looking at an LCD 
display attached to a printed circuit board (PCB). Unscrew the LCD from the PCB. There will be some 
form of connection between the LCD and the 16 pins on the PCB (Figure 10-5). In our case, it was 
connective rubber that we kept for future repositioning. 

The ultimate goal here is to acquire and decode the information sent from the PCB to the LCD, so 
you solder one wire to each of the LCD pins. (Later you will try to figure out which ones define the eight 
bits of information relevant to you.) Use flexible wrap wire or a 16-wire ribbon cable, preferably; hook- 
up wire is too sturdy and could strip the PCB connectors off. 

If there are connections on the PCB that you can use without impeding the repositioning of the 
LCD, use those so you can still read the numbers on your scale as depicted in Figure 10-6. This will be 
extremely helpful when analyzing the information from the pins. 


st @ - 


Figure 10-5. Main PCB with LCD pin connections (at the top) 
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Test that all pins are properly connected and there is no electrical flow between them. Then replace 
the LCD and the scale cover, leaving all the LCD wires and the analog cable you previously connected 
outside. You will probably need to carve an indent on the plastic cover with a scalpel so you can pass the 


wires through. Connect all the cables to an Arduino prototyping shield so they are ready to be easily 
plugged into your board (Figures 10-6 through 10-8). 
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Figure 10-7. Arduino and XBee module plugged and LCD repositioned 
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Figure 10-8. Arduino Prototype Board connected to all the LCD pins and the PCB 


Add an XBee module to the board, as explained in Chapter 7, and put everything into a transparent 
plastic box along with an external power supply (Figure 10-9). This way, you won’t need to depend on 
the USB cable’s length to use your scale. 


Figure 10-9. Scale finished and plugged to the Arduino and XBee module 
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Acquiring the LCD Signal 


You have a pretty exciting cryptographic exercise ahead. When you connect your shield to the Arduino 
and read the values coming from the LCD pins, you will be overwhelmed by a constant flow of ones and 
zeros passing through your Serial Monitor. Don’t despair; you are going to make sense of all this in the 
next few pages! 

Connect your Arduino to your computer and start a new Arduino sketch. If you found a breakout 
header like the one shown previously, you might be able to power your scale from the Arduino; connect 
the pins to ground and 3.3V so you can get by without using a battery. 

By now you should be acquainted with Arduino code, so you should have no problem reading 
through the following code. 


#define dataSize 16 
int data [dataSize]; 


void setup() { 
Serial. begin(9600) ; 
pinMode(2, INPUT) ; 
pinMode(3, INPUT) ; 
pinMode(4, INPUT) ; 
pinMode(5, INPUT) ; 
pinMode(6, INPUT) ; 
pinMode(7, INPUT) ; 
pinMode(8, INPUT) ; 
pinMode(9, INPUT) ; 
pinMode(10, INPUT) ; 
pinMode(11, INPUT) ; 
pinMode(12, INPUT) ; 


} 


void loop() { 
for(int i = 
data[i-2] 

} 
data[11] 
data[12] 
data[13] 
data[14] 
data[15] 


23 i<13; i++){ 
= digitalRead(i); 


digitalRead(A5); 
digitalRead(A4) ; 
digitalRead(A3); 
digitalRead(A2); 
digitalRead(A1) ; 


for(int i = 0; i<dataSize; it++){ 
Serial. print(data[i]); 


Serial.print1n(); 
You are defining your pins as inputs (only the digital ones) and then storing their current digital 
readings in the array of integers you have called data. Note that the order is defined by the connections 


you used. In your Arduino, you connected the LCD pins left to right from Arduino pin 2 to 12 and then 
from analog 5 to 1, hence the order of the readings. 
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You then print the resulting array to your serial port. If you open the Serial Monitor, you will be 
confronted with something similar to the following lines: 


0000000011100000 
1000000000000000 
0000000001010000 
0010000000000000 
0000000011110000 
0000000011100000 
0100000000000000 


This won't really make any sense to you yet, but you can start to identify a pattern. There are a series 
of sparse ones written on the left side of the string, and then some more tightly grouped at the center- 
right. Not much, but it’s a start. 


Reading the LCD Signal 


Every line of code seems to be different, but an LCD should keep the pins high for a while, turn them off, 
and them on again for a certain time. Arduino should be faster than that, so you should get at least a 
couple of consecutive lines that look the same every time. You might have guessed the answer already: 
Arduino can indeed loop faster than the LCD cycle, but the serial printing slows it down considerably, so 
you don’t get the same reading twice. Let’s test this with some more sophisticated code. 

You defining a bi-dimensional array named serial that will store fifty lines of LCD signals. 
Whenever it acquires the fifty lines, it prints them to the Serial Monitor. This way, you're not slowing 
down your Arduino every cycle, so you should be able to gather the information faster. When you print 
the information out, you will, of course, spend some time and certainly skip some cycles, but you will 
still have a consistent stream of data in your serial array. 


#define dataSize 16 

int data [dataSize]; 

#define lines 50 

int serial [lines][dataSize]; 
int index; 


void setup() { 
Serial. begin(9600) ; 
pinMode(2, INPUT) ; 
pinMode(3, INPUT) ; 
pinMode(4, INPUT) ; 
pinMode(5, INPUT) ; 
pinMode(6, INPUT) ; 
pinMode(7, INPUT) ; 
pinMode(8, INPUT) ; 
pinMode(9, INPUT) ; 
pinMode(10, INPUT) ; 
pinMode(11, INPUT) ; 
pinMode(12, INPUT) ; 


} 


void loop() { 
for(int i 


= 25 i<13; itt){ 
data[i-2] = 


digitalRead(i); 
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data[11] = digitalRead(A5); 
data[12] = digitalRead(A4); 
data[13] = digitalRead(A3); 
data[14] = digitalRead(A2); 
data[15] = digitalRead(A1); 


for(int i = 0; i<dataSize; i++){ 
serial[index][i] = data[i]; 


index++; 
if (index==lines){ 
index=0; 
for(int i = 0; i<lines; i++){ 
for(int j = 0; j<dataSize; j++){ 
Serial. print(serial[i][j]); 


Serial.print1n(); 


Serial. print1n(" ")5 
} 
} 


If you upload this sketch and open the Serial Monitor, you will get a surprisingly different 
panorama: 


0000000001010000 
0000000001010000 
0000000001010000 
0000000011110000 
0000000011110000 


1000000000000000 
1000000000000000 
0000000000000000 
0010000000000000 
0010000000000000 


You can see that the LCD pins are sending a pattern for a time and then changing to another 
pattern. You can’t synchronize your Arduino to the LCD, but you can write code that reads the numbers 
only if they have changed and if they are maintained for at least two cycles (to avoid misreading at the 
transition point). 


#define dataSize 16 

int data [dataSize]; 

int previousData [dataSize]; 
#define lines 50 

int serial [lines][dataSize]; 
int index; 


// Booleans defining the change 
boolean changed; 
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boolean scanning; 


void setup() { 
Serial. begin(9600) ; 
pinMode(2, INPUT) ; 
pinMode(3, INPUT) ; 
pinMode(4, INPUT) ; 
pinMode(5, INPUT) ; 
pinMode(6, INPUT) ; 
pinMode(7, INPUT) ; 
pinMode(8, INPUT) ; 
pinMode(9, INPUT) ; 
pinMode(10, INPUT) ; 
pinMode(11, INPUT) ; 
pinMode(12, INPUT) ; 


} 


void loop() { 
for(int i = 
data[i-2] 

} 
data[11] 
data[12] 
data[13] 
data[14] 
data[15] 


23 i<13; i++){ 
= digitalRead(i); 


digitalRead(A5); 
digitalRead(A4); 
digitalRead(A3) ; 
digitalRead(A2) ; 
digitalRead(A1) ; 


Check if the numbers have changed from the previous reading 


int same = 0; 

for(int i = 0; i<dataSize; i++){ 
if (data[i]==previousData[i])same++; 
previousData[i] = data[i]; 


if (same<dataSize) { 
changed=true; 
scanning=true; 


else{ 
changed=false; 


} 


If the numbers have NOT changed and if they have occurred at least twice (so scanning is true), 
store the the data into the serial[] array. 


if(changed == false && scanning == true){ 
for(int i = 0; i<dataSize; i++){ 
serial[index][i] = data[i]; 


index++; 
scanning=false; 


} 
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Finally, if you have reached the buffer limit, reset the index to 0 and run a for loop to print each line 
in your serial[] array to serial. 


if (index==lines){ 
index=0; 
for(int i = 0; i<lines; i++){ 
for(int j = 0; j<dataSize; j++){ 
Serial. print(serial[i][j]); 


Serial.print1n(); 


} 


Serial. println(" ")3 


This will actually give you the key to reading the scale’s LCD signals. If you open your Serial 
Monitor, you now see a clear pattern in the flow of data streaming from the scale. 


1000000000000000 
0010000000000000 
0001000000000000 
0000000011100000 
0000000011110010 
0000000001010000 
0000000011110000 
0100000000000000 


This is your data stream when the scale is showing a 0.0 kg signal. If you settle on another weight, 
the signal will look differently. Let’s try 67.4kg. 


1000000000000000 
0010000000000000 
0001000000000000 
0000001001000000 
0000001101010010 
0000001001110000 
0000001111100000 
0100000000000000 


Well, that’s good; you can now see that there is a pattern that repeats itself at the far-left of your 
strings, and then some numbers that change at the center. The ones appearing on the right side are 
actually defining if the kg or |b symbol is turned on. 

After quite a lot of testing, we were able to identify the pins that carried the information on the digits 
and the ones that dealt with other elements. You should run a similar trial and error process to find out 
the order in your particular scale. This was the one in ours. 


Pin Oo 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 
100 0 {| 0 o | 0 o | 0 oO | 0 0 | 0 0 0 0 
0 01 0 ] 0 0 | 0 oO | 0 O | 0 O | 0 0 0 O 
0 0 oo 1 | 0 0 | 0 O | 0 O | O0 O | 0 0 0 O 


The first three rows don’t change with the weight, so they can be used to identify the start of the 
message when pin 3 is turned on. 
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0 0 0 0 |] 0 o | 14 0 | O 14 | 0 0 | 0 0 0 O 
0 0 0 0 | 0 o | 4 14 | oO 14 | O 1 | O 0 1 +0 
0 0 0 0 | 0 o | 14 0 | oO 4 | 4 124 | 0 0 0 O 
0 0 oo | 0 o | 4 4 | 4 2 | 2 0 | 0 0 0 O 


After the three first rows have passed, the fifth to eighth rows carry the important information. 
Columns 4-5 each consist of 8 bits, defining the hundreds. Columns 6-7 define the tens, 8-9 the units, 
and 10-11 the decimals. Column 14 provided information on the unit used (kg or lbs). 


0 100 ;] 0 0 | 0 0 | 0 O | 0 O | 0 0 0 0 


The last row is always constant and can be used to identify the end of the message. 


Sending the Signal to Processing 


You have acquired the information sent to the LCD display. You now want to decode it so you can work 
with the weight. You are going to send the raw information to Processing and write a decoder to 
transform this information into numbers. 

Stop scanning columns 12, 13, and 15 because they don’t add any useful information. Write a 
routine that checks the start of the message (one on column 3) and the end of the message (one on 
column 2) and sends to serial all the rows in between. Also send the information coming from analog pin 
0 connected to the PCB’s analog output that changes with the state of the weighing process. 


#define dataSize 13 

int data [dataSize]; 

int previousData [dataSize]; 
#define lines 4 

int serial [lines][dataSize]; 
int index; 

// Booleans defining the change 
boolean changed; 

boolean scanning; 

boolean start; 

int state; 

int previousState; 


void setup() { 
Serial. begin(9600) ; 
pinMode(2, INPUT) ; 
pinMode(3, INPUT) ; 
pinMode(4, INPUT) ; 
pinMode(5, INPUT) ; 
pinMode(6, INPUT) ; 
pinMode(7, INPUT) ; 
pinMode(8, INPUT) ; 
pinMode(9, INPUT) ; 
pinMode(10, INPUT) ; 
pinMode(11, INPUT) ; 
pinMode(12, INPUT) ; 


} 
void loop() { 


CHAPTER 10 


for(int i 


= 25 1i<13; it+){ 
data[i-2] = 


digitalRead(i); 


} 

data[11] = digitalRead(A5); 
data[12] = digitalRead(A2); 
state = analogRead(A0); 


// Check if the numbers have changed from the previous reading 
int same = 0; 
for(int i = 0; i<dataSize; i++){ 
if(data[i]==previousData[i])same++; 
previousData[i] = data[i]; 


if (same<dataSize) { 
changed=true; 
scanning=true; 


else{ 
changed=false; 


} 


if(changed == false && scanning == true){ 
if(data[1] == 1){ 
start=false; 
Serial.print1n('S'); 
for(int i = 0; i<4; i++){ 
for(int j = 0; j<dataSize; j++){ 
Serial.print(serial[i][j]); 


Serial. print1n(); 
} 
Serial.print('X'); 
Serial.print1n(state) ; 


if(start){ // If you have started the message 
for(int i = 0; i<dataSize; i++){ 
char number[1]; 
// Serial.print(itoa(data[i], number, 10)); 
// Serial.print(" "); 
serial[index][i]=data[i]; 


index++; 

} 

scanning=false; 

if(data[3] == 1){ 
start=true; 
index = 0; 

} 

} 
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This code will output the following data stream to the serial buffer: 


S 
0000000001100 
0000000001111 
0000000011110 
0000000010110 
X532 

S 
0000000001100 
0000000001111 
0000000011110 
0000000010110 
X532 


You have enough data to identify the start and end of the information package, and all the necessary 
data to recompose the weight information at the other end of the line. 


Decoding the LCD Signal 


The decoding process is far simpler than the acquisition of the information. At this point, you can easily 
write a Processing program that receives the information from Arduino and decodes it into numbers you 
can understand. You are going to pack this process into a class so you can reuse the implementation in 
different applications. 

This class is called LCD. You input a two-dimensional array defining the individual number on the 
seven-segment display and you get a float defining the weight. 


public class LCD { 
private float data; 
int lIcdArray[][] = new int[4][13]; 
private String units = "kg"; 
private int state; 
boolean foundWeight; 
boolean scanning; 


int[] zero = { 1, 1, 1, 0, 1, 1, 1 }; 
int[] one = { 0, 0, 1, 0, 1, 0, 0 }; 
int[] two = {dy yO} A545 05 1} 5 
int[] three = { 1, 0, 1, 1, 1, 0, 1 }; 
int[] four = { 0, 0, 1, 1, 1, 1, 0 }; 
int[] five = { 1, 0, 1, 1, 0, 1, 1 }; 
int[] six = { 1, 1, 1, 1, 0, 1, 1 }; 
int[] seven = { 0, 0, 1, 0, 1, 1, 1 }; 
int[] eight = fas. Vo 13 es 4d. fs 
int[] nine = {250552545 4572, oh 3 
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Figure 10-10. Seven-segment display code 


The arrays you just defined are a blueprint to check your data against. They are based on the 
distribution of the seven-segment bits coming from Arduino. After, again some intensive testing 
sessions, we found the pattern depicted in Figure 10-10. 

The method getReading() returns the real value of the weight. 


public float getReading() { 
return data; 


} 
Use the method setLcdArray() to input the new values from Arduino into your LCD class to keep 
the numbers updated. You convert every character to an integer, store them in the lcdArray, and run the 
update() method to update your real value. 


public void setLcdArray(char[][] stringData) { 
if (stringData[1][12] == '1') { 


units = "kg"; 
else if (stringData[2][12] == '1') { 
units = "1b"; 


for (int i = 0; i < stringData.length; i++) { 
for (int j = 0; j < stringData[o].length; j++) { 
IcdArray[i][j] = Character.digit(stringData[i][j], 10); 


} 
this.update(); 


Within the update function, you reorganize your main two-dimensional array into a three- 
dimensional array that stores the four two-dimensional arrays defining each of the digits. 


public void update() { 
int[] digits = new int[4]; 
int[][][] segments = new int[4][4][2]; 


for (int i= 0; i< IcdArray. length; i++) { 
for (int j = 45 j < 6; j++) { 
segments[o][i][j - 4] = IcdArray[i][j]; 


for (int j = 6; j < 8; j++) { 
segments[1][i][j - 6] = lcdArray[i][j]; 


for (int j = 8; j < 10; j++) { 
sepnentel2 its - ie IcdArray[i][j]; 
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for (int j = 10; j < 12; j++) { 
segments[3][i][j - 10] = lcdArray[i][j]; 


} 


Then you use the function getNumber(), passing each of the bi-dimensional arrays and thus getting 
an array of the individual numbers displayed on the LCD. 


for (int i = 0; i < digits.length; i++) { 
digits[i] = getNumber(segments[i]); 


Lastly, you recompose the individual digits into the real value of the weight by multiplying the 
hundreds by 100, the tens by 10, the units by 1, and the decimals by 0.1. This real value is stored in the 
float variable data. 


data = digits[o] * 100 + digits[1] * 10 + digits[2] + digits[3] * 0.1; 


You use the function getNumber() to get the decimal number represented by each of your seven- 
segment bi-dimensional arrays. This process starts by extracting the seven relevant bits of the incoming 
array and storing them in a linear array. You compare this array to each one of the number blueprints 
that you defined previously. If it matches one of them, the corresponding number will be returned. 


public int getNumber(int segments[][]) { 
int flatSegment[] = new int[7]; 
for (int i = 0; i < segments.length; i++) { 
for (int j = 0; j < segments[0].length; j++) { 
if (i + j == 0) { 
flatSegment[i * 2 + j] = segments[i][j]; 


} 
else if (!(i == 0 && j == 1)) { 
flatSegment[i * 2 + j - 1] = segments[i][j]; 


, } 
if (Arrays.equals(flatSegment, zero)) { return 0; } 


else if (Arrays.equals(flatSegment, one)) { return 1; } 
else if (Arrays.equals(flatSegment, two)) { return 2; } 
else if (Arrays.equals(flatSegment, three)) { return 3; } 
else if (Arrays.equals(flatSegment, four)) { return 4; } 
else if (Arrays.equals(flatSegment, five)) { return 5; } 
else if (Arrays.equals(flatSegment, six)) { return 6; } 
else if (Arrays.equals(flatSegment, seven)) { return 7; } 
else if (Arrays.equals(flatSegment, eight)) { return 8; } 
else if (Arrays.equals(flatSegment, nine)) { return 9; } 
else { 
return 0; 
} 


} 


The setState() function is used to set the state of the scale from the main sketch. You perform a 
check of the state of the scale and set the scanning and foundWeight Boolean values accordingly. 


public void setState(int state) { 
this.state = state; 
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if (state < 518 &8& state > 513) { 
scanning = true; 


if (state < 531 && state > 526 && scanning) { 
foundWeight = true; 


The following function displays the raw data in array form on screen. 


public void displayArray(int x, int y) { 
pushMatrix(); 
translate(x, y); 
for (int i = 0; i < IcdArray[0].length; i++) { 
for (int j = 0; j < IcdArray.length; j++) { 
text(1cdArray[j][i], 20 * i, j * 20); 


eran 
} 


Note that the last curly brace the closing curly brace for the whole class. 


Using the Weight Data 


You are going to write a first simple sketch to test the decoding process. Then you will use it with your 
Kinect data, so let’s check that it works properly before starting to make things more complex. 

Start a new sketch and create a separate tab in which you paste the LCD class. Then go back to the 
main tab and add all the necessary code to acquire serial communication. Then declare and initialize the 
LCD object, and declare a character array to store the incoming data. 


import processing.serial.*; 

Serial myPort; 

PFont font; 

LCD lcd; 

char[][] stringData = new char[4][13]; 
int currentLine; 


public void setup() { 
size(400, 300); 
font = loadFont("SansSerif-14.vlw"); 
textFont(font) ; 
String portName = Serial.list()[0]; // This gets the first port on your computer. 
myPort = new Serial(this, portName, 9600); 
myPort.bufferUntil('\n'); 
lcd = new LCD(); 


The main draw() function updates the LCD with the latest data from Arduino, displays the array 
data, and draws the result weight on the screen, as shown in Figure 10-11. 


public void draw() { 
background(0) ; 
lcd.setLcdArray(stringData) ; 
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lcd.displayArray(50, 50); 
text(1cd.data + " " + lcd.units, 50, 200); 
} 


B20 _1_Weight_Decoder 


Figure 10-11. Testing the LED data decoder 


You have worked with the serialEvent() function previously in this book, so it shouldn’t be 
completely unknown to you. Trim the newline character from your incoming string, and then check 
whether you have received a start or end of communication character. 


public void serialEvent(Serial myPort) { 
String inString = myPort.readString(); 
inString = inString.substring(0, inString.indexOf('\n') - 1); 


If you receive an S, set the current line to 0, so all new data is stored on the first line of your data 
array. 


if (inString != null) { 
if (inString.equals("S")) { 
currentLine = 0; 


} 


If the first character of your incoming string is an X, this means you are done with this reading. The 
numbers following the X are the reading of your LCD state. Pass that value to the LCD object. 


else if (inString.charAt(0) == 'X') { 
String newState = inString.substring(1); 
lcd. setState(Integer.valueOf(newState) ) ; 
} 


Otherwise, check if the length of the array is correct, that you are not beyond the fourth line, and 
store the values of the incoming characters into the stringData array. 


else { 
if (inString.length() == 13 && currentLine<4) { 
for (int i = 0; i < stringData[o].length; i++) { 
stringData[currentLine][i] = inString.charAt(i); 
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} 
currentLine++; 
} 
} 


else { 
// increment the horizontal position: 
println("No data to display"); 


And that’s all! When you run this you should see the array of ones and zeros on the screen defining 
the LCD pins information and the real value of the weight displayed on the scale. You’re done with the 
weight data acquisition; let’s have fun with it and your Kinect data! 


Implementing the Biometric Recognition 


You have probably had enough of playing with your scale by now, so youre going to plunge straight into 
the final piece of code that will use skeleton tracking to recognize users and the weight input to generate 
body mass index (BMI) values. The basic functioning of the program is the following; whenever a user 
enters the field of vision of Kinect, you start tracking him. Find the highest and lowest points defining 
your user and extract the height of the person from those points. At the same time, you skeletonize the 
user and extract the proportions of their limbs to use as a footprint to match the user with previously 
saved information. If the footprint is similar to an existing user, you recognize him and retrieve the data 
from previous readings. You also have an input interface that allows you to create new users, stating 
their names and dates of birth. Once you have all of the skeleton and height data, the user steps on the 
bathroom scale and you read his weight. You then save all the current information in an external CSV file 
and calculate the person’s BMI and display it on screen. 


Note A CSV (comma-separated values) file is a file format that stores tabular data separated by commas. It is 
used extensively in the transfer of data between applications. 


Controlling the height and weight of children from early life up to adulthood can be a good way to 
keep track of adequate development, so for the sake of this exercise, compare your data to the male 
BMI-for-age chart for children less than 20 years old. You can find this chart at 
http://www.cdc.gov/growthcharts/clinical_charts.htm. This chart applies to US citizens; you can 
probably find equivalent charts from your government’s health program. 


Imports and Variable Declaration 


The first thing you need to do in your Processing sketch is to import the external libraries that you need 
for this process. You are building a basic user interface, so import Java AWT and AWT. event, which are 
part of the Java Foundation Classes and allow you to interact with your sketch in runtime by introducing 
new users. 


import processing.serial.*; 
import SimpleOpenNI.*; 
import java.awt.*; 
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import java.awt.event.*; 


SimpleOpenNI kinect; 
Serial myPort; 

PFont font; 

PFont largeFont; 
LCD lcd; 


You are going to be juggling quite a few external files, so next you define an array called files that 
stores all the previous user data files that exist in the data folder. You also declare a current user file, two 
AWT TextFields, and one button for the user interface. 


// User Input and Files 

File[] files; // Existing user files 
File userFile; // Current user 
TextField name; 

TextField birthdate; 

Button b = new Button("New User"); 


The serial variables are similar to previous examples. The stringData array stores the incoming 
information from Arduino and one integer, currentLine, which you use in the process of parsing the 
incoming data. The Boolean scan is set to true, and then it is changed to false once you have acquired 
the final weight information. 


// Serial Variables 

boolean serial = true; 

char[][] stringData = new char[4][13]; 
int currentLine; 

boolean scan = true; 


The numeric user data is stored in the array of floats userData. You also store some user information 
in String format, such as the name and date of birth. 


// User Data 

float[] userData = new float[11]; 
int user; // Current user ID 

String userName = "new User"; 

String userBirthDate = "00/00/0000"; 
int userAge; 

float userBMI; 

float userHeightTemp; 


You declare a double array to keep the data in the chart that you want to display, and a File object 
defining the chart CSV file. The String today contains the current date. 


// Charts 
float[][] chart; 
File chartFile; 
String today; 


Setup Function 


The setup() function is slightly longer than usual due to the user interface elements and the objects 
managing the external files. The first half of the function deals with the Simple-OpenNI object and the 
Processing fonts. 
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Along with the depthMap and the user capabilities, you enable the scene from NITE. The scene allows 
you to separate the background from the foreground user. This means you can select the 3D points that 
make up the user’s volume and extract dimensions from it, namely the user height (Figure 10-12). 


Figure 10-12. Kinect scene: the user is automatically recognized. 


public void setup() { 
size(1200, 480); 
smooth(); 


kinect = new SimpleOpenNI (this) ; 
kinect.setMirror(true) ; 

kinect.enableDepth() ; 

kinect.enableScene(); 
kinect.enableUser(SimpleOpenNI.SKEL_PROFILE ALL); 


font = loadFont("SansSerif-12.vlw"); 
largeFont = loadFont("SansSerif-14.vlw") ; 
textFont (largeFont) ; 

lcd = new LCD(); 


if (serial) { 
String portName = Serial.list()[0]; // This gets the first port 
myPort = new Serial(this, portName, 9600); 
myPort.bufferUntil('\n'); // don't generate a serialEvent() unless you get a newline 


} 


User Interface 


Now you initialize the AWT objects that constitute your user interface (Figure 10-13). The input is based 
on two Java AWT TextFields and one button. You actually position these three elements out of the 
Processing frame, right under it. When one user is detected, you resize the height of the Processing 
frame, but not the applet, so you then expose the part of the frame containing the interface. The two text 
fields contain the name and date of birth of the new user. 
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Your Name : af ‘Date of Birth dd/mm/yyyy New User 


Figure 10-13. User interface 


// Setting Up the User Input Elements 

name = new TextField(40); 

birthdate = new TextField(40); 
frame.add(name) ; 

frame.add(birthdate) ; 

frame.add(b); 

name.setBounds(20, height + 27, 200, 20); 
name.setText("Your Name"); 
birthdate.setBounds(260, height + 27, 200, 20); 
birthdate.setText("Date of Birth dd/mm/yyyy"); 
b.setBounds(520, height + 27, 100, 20); 


You need to add an action listener to your button so an action is triggered when you press it. In your 
case, you store the user name and date of birth and you call the function newUser(), passing these 
strings as parameters. 


b.addActionListener(new ActionListener() { 
public void actionPerformed(ActionEvent e) { 
userName = name.getText(); 
userBirthDate = birthdate.getText(); 
newUser(userName, userBirthDate) ; 


} 
)3 


You want to be able to recognize users created in previous sessions. For this, you need to scan the 
data folder in search of CSV files starting with the characters “user_”. Use Java’s FilenameFilter for this 
purpose. The following lines first get a list of all files in the data folder and then apply the filter and store 
the resulting files into the File array files. You then print the file names to the console for debugging. 


// Acquire the user files already on the folder 
File directory = new File(dataPath("")); 
FilenameFilter filter = new FilenameFilter() { 
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public boolean accept(File dir, String name) { 
return name.startsWith("user_"); 
} 
5 
files = directory.listFiles(filter) ; 
println("List of existing user files"); 
for (File f : files) { 
print1n(f); 


To finish the setup() function, you choose the chart you are going to be comparing your results to 
and trigger the function readChart(), which retrieves all the data from the file. You also get the current 
date with the function getDate(). 


// Acquire the chart file 

chartFile = new File(dataPath("charts/Male 2-20 BMI.CSV")); 
readChart (chartFile); 

getDate(); 


Draw Function 


The draw() loop, in contrast to the setup() function, is quite simple because you have implemented the 
main routines into separate functions that are detailed next. The first thing you do in each loop is to 
update the data from the Simple-OpenNI and LCD objects. 


public void draw() { 
kinect.update(); 
background(0) ; 
lcd. setLcdArray(stringData) ; 
image(kinect.sceneImage(), 0, 0); 


Then, if you have detected a user, you update the height data, as you are taking it from the user 
point cloud. 


if (kinect.getNumberOfUsers() != 0) { 
updateUserHeight (); 


} 


If you are tracking a skeleton as well, you draw it on screen and update the user data from his limbs 
proportions. 


if (kinect.isTrackingSkeleton(user)) { 
drawSkeleton(user) ; 
updateUserData(user) ; 


} 


Then, if you are still scanning the weight (scan is true), and the LCD object’s variable foundWeight is 
true (meaning you got a steady weight), you try to recognize the user from previous users, and then you 
store the first piece of data (the weight from the scale) in the first position of your userData array and the 
height in its second position. You then update the BMI value and save the user for future retrieval. You 
set the scan Boolean to false so you stop updating the values. 


if (1cd.foundWeight && scan) { 
userRecognition(); 
userData[0] = lcd.data; 
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userData[1] = userHeightTemp; 

userBMI = userData[0]/pow(userData[1] /1000, 2); 
saveUser(); 

scan = false; 


And finally, you take care of the visualization by printing the chart and the user data on the right 
side of your Processing screen. 


printUserData(); 
drawChart(); 


Additional Functions 


You probably guessed it: a short draw loop means a quite extensive list of additional functions! Some of 
these functions are very similar to code used in previous examples, so we won’t go into detail on those. 
Others are more complex and will be described exhaustively, explaining what they do and when they 
are called. 

Before you get into the user functions, there is a helper function called getDate() that uses the Java 
Calendar class to fetch the current date and store it in the variable today. It also calculates the age of the 
current user by subtracting the user’s date of birth from the current date. 


public void getDate() { 
Calendar currentDate = Calendar.getInstance(); 
SimpleDateFormat formatter = new SimpleDateFormat("dd/MM/yyyy") ; 
today = formatter. format(currentDate.getTime()); 
Date todayDate = currentDate.getTime(); 
Date userDate = null; 
try { 
userDate = formatter.parse(userBirthDate) ; 


catch (ParseException e) { 
e.printStackTrace(); 


} 
long diff = todayDate.getTime() - userDate.getTime(); 
userAge = (int) (diff/315569260001); 


New User 


The newUser() function is called whenever you press the New User button in your user interface. The 
user name and date of birth are passed as arguments. 

You first need to check if you have pressed the button without entering a name or a date, in which 
case you prompt the user to insert the data. Then, you use the Java function DateFormat () to check 
whether the date that has been inserted corresponds to the format you are expecting. If it doesn’t, a 
parse exception is thrown and caught by your try-catch statement, and you prompt the user to insert a 
properly formatted date of birth (which doesn’t mean it is the real one anyway...). 

If the data is correct, you proceed to check whether there is already a user with that name in the 
system, in which case you simply use that file for the rest of the time. If it doesn’t exist, you create it and 
add the user name and date of birth to the first line in the file. You finally flush and close the 
PrintWriter. 
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public void newUser(String name, String birth) { 
// Start the user file 
if (name.equals("Your Name") || birth.equals("Date of Birth dd/mm/yyyy")) { 
println("Please insert your name and date of Birth"); 


else { 


DateFormat format = new SimpleDateFormat("dd/MM/yyyy") ; 


try { 


format .parse(birthdate.getText()); 
PrintWriter output; 


userFile = new File(dataPath("user_" + userName + 


".csv")); 


if (!userFile.exists()) { 
output = createWriter(userFile) ; 
output.println(name + "," + birth); 
output.flush(); // Write the remaining data 
output.close(); // Finish the file 
getDate(); 
println("New User " + name + " created successfully"); 


else { 
setPreviousUser(userFile) ; 


} 


catch 


(ParseException e) { 


System. out 
-println("Date " + birthdate. getText() 


+ 
+ 
+ 


} 
} 
i 


is not valid according to 
((SimpleDateFormat) format) .toPattern() 
" pattern."); 


Updating User Data 


You call updateUserData() from the draw() loop in every cycle if you are tracking a skeleton. This 
function first stores all the real world coordinates of the user’s joints; then it calculates the dimensions of 
arms, forearms, thighs, shins, and shoulder breadth; and then it stores all of this data in the userData 
array, Starting from the third position (the first two are occupied by the weight and height). 

This array serves later as an identifier for the user. The body proportions stored in the userData array 
can be matched and compared to other users’ identifiers and thus the person tracked can be recognized. 
The first step is to initialize all the PVectors that store the location of the user’s joints. 


void updateUserData(int id) { 


// Left 
PVector 
PVector 
PVector 


// Left 
PVector 
PVector 
PVector 


Arm Vectors 

lHand = new PVector(); 
1Elbow = new PVector(); 
1Shoulder = new PVector(); 


Leg Vectors 

1Foot = new PVector(); 
1Knee = new PVector(); 
lHip = new PVector(); 
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// Right Arm Vectors 

PVector rHand = new PVector(); 
PVector rElbow = new PVector(); 
PVector rShoulder = new PVector(); 


// Right Leg Vectors 

PVector rFoot = new PVector(); 
PVector rKnee = new PVector(); 
PVector rHip = new PVector(); 


Then you use the getJointPositionSkeleton() function to get the real world coordinates into the 


vectors. 


// Left Arm 

kinect.getJointPositionSkeleton(id, 
kinect.getJointPositionSkeleton(id, 
kinect.getJointPositionSkeleton(id, 
// Left Leg 

kinect.getJointPositionSkeleton(id, 
kinect.getJointPositionSkeleton(id, 
kinect.getJointPositionSkeleton(id, 
// Right Arm 

kinect.getJointPositionSkeleton(id, 
kinect.getJointPositionSkeleton(id, 
kinect.getJointPositionSkeleton(id, 
// Right Leg 

kinect.getJointPositionSkeleton(id, 
kinect.getJointPositionSkeleton(id, 
kinect.getJointPositionSkeleton(id, 


SimpleOpenNI. 
SimpleOpenNI. 
SimpleOpenNI. 


SimpleOpenNI. 
SimpleOpenNI. 
SimpleOpenNI. 


SimpleOpenNI. 
SimpleOpenNI. 
SimpleOpenNI. 


SimpleOpenNI. 
SimpleOpenNI. 
SimpleOpenNI. 


SKEL_LEFT_HAND, 1Hand) ; 
SKEL_LEFT ELBOW, 1Elbow); 
SKEL_LEFT SHOULDER, 1Shoulder) ; 


SKEL_LEFT FOOT, 1Foot); 
SKEL_LEFT KNEE, 1Knee); 
SKEL_LEFT HIP, lHip); 


SKEL_RIGHT HAND, rHand); 
SKEL_RIGHT ELBOW, rElbow); 
SKEL_RIGHT SHOULDER, rShoulder) ; 


SKEL_RIGHT FOOT, rFoot); 
SKEL_RIGHT KNEE, rKnee); 
SKEL_RIGHT HIP, rHip); 


And finally, you store the distances between certain joints in the userData array. 


userData[2] = PVector.dist(rHand, rElbow); 
userData[3] = PVector.dist(rElbow, rShoulder); 
userData[4] = PVector.dist(lHand, lElbow); 
userData[5] = PVector.dist(lElbow, 1Shoulder); 
userData[6] = PVector.dist(1Shoulder, rShoulder) ; 
userData[7] = PVector.dist(rFoot, rKnee); 
userData[8] = PVector.dist(rHip, rKnee); 
userData[9] = PVector.dist(1Foot, 1Knee); 


userData[10] = PVector.dist(lHip, 1Knee); 


Updating User Height 


The detection of the user’s height is not based on skeleton tracking but directly on the point cloud, so 
you implement a separate function for it. This function is called from draw in case you're tracking a user 


in the scene. 


// Right Arm Length 
// Right Upper Arm Length 
// Left Arm Length 
// Left Upper Arm Length 
// Shoulder Breadth 
// Right Shin 
// Right Thigh 
// Left Shin 
// Left Thigh 


You have previously worked with functions that parsed and printed on screen the Kinect point 


cloud. This function is similar, but you won’t print any of the points. What you want to do is to find out 
the higher and lower points from the user point cloud so you can subtract their Y coordinates and thus 


find the real height of the user in millimeters (Figure 10-14). 
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Figure 10-14. User height from highest and lowest points 


You will only use one of every three points to speed up the process; if you want a more precise 
measurement, you can decrease the steps to 1. Use userMap from Simple-OpenNI to get a flat image of 
the points that define the user. 


void updateUserHeight() { 
int steps = 3; 
int index; 
int[] userMap = kinect.getUsersPixels(SimpleOpenNI.USERS ALL); 
PVector userCenter = new PVector(); 
PVector[] realWorldPoint = new PVector[kinect.depthHeight() * kinect.depthwWidth() ]; 


Then you initialize the userTop and userBottom PVectors to the center of the mass of the user, so you 
can start looking for a top and bottom pixel from a neutral ground. 


kinect.getCoM(1, userCenter); // Get the Center of Mass 
PVector userTop = userCenter.get(); 
PVector userBottom = userCenter.get(); 


And you run a loop checking every point in the depth map looking for user points. You determine 
that a point belongs to a user if the equivalent pixel in the userMap is not black. If you find a user point, 
you check if it’s higher than the higher pixel found or lower than the lowest pixel found; in positive case, 
you set it as the highest or lowest point. 


for (int y = 0; y < kinect.depthHeight(); y += steps) { 
for (int x = 0; x < kinect.depthWidth(); x += steps) { 
index = x + y * kinect.depthWidth(); 
realWorldPoint[index] = kinect.depthMapRealWorld()[index].get(); 
if (userMap[index] != 0) { 
if (realWorldPoint[index].y > userTop.y) 
userTop = realWorldPoint[index].get(); 
if (rvealWorldPoint[index].y < userBottom.y) 
userBottom = realWorldPoint[ index] .get(); 
} 


} 
} 
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Once you have found the highest and lowest points, you store the value of the difference of their Y 
coordinates into userHeightTemp. But you won’t do it straight away. You want to smooth the noisy data 
to avoid continuous oscillations of the value, so you use Processing’s lerp() function to progressively 
make the transition from one value to the next one. 


userHeightTemp = lerp(userHeightTemp, userTop.y - userBottom.y, 0.2f); 


Saving the User 


If you have successfully created a user and tracked his weight, height, and proportions, now you want to 
store this information into the previously created CSV file (Figure 10-15). 

For this, you create a File object with the name of the current user, and if you find that it doesn’t 
exist yet, you prompt the user to insert his name and birth date. If the file does exist, use a Java 
BufferedWriter to append information to the file without wiping it out. 


Enrique 12/11/1982 
29/11/2011 63.0 1686.2014 268.18143 227.60263 263.1212 236.44228 279.46335 305.47165 396.49283 270.1577 393.8149 


Figure 10-15. Structure of the user information CSV file 


private void saveUser() { 
userFile = new File(dataPath("user_ 
if (!userFile.exists()) { 
println("Please Enter your Name and Date of Birth"); 
} else { 
us 9 ge 
FileWriter writer = new FileWriter(userFile, true); 
BufferedWriter out = new BufferedWriter(writer) ; 
out .write(today) ; 
for (int i = 0; i < userData.length; i++) { 


out.write("," + userData[i]); 


+ userName + ".csv")); 


out.write("\n"); 
out.flush(); // Write the remaining data 
out.close(); // Finish the file 


} 
catch (I0Exception e) { 
e.printStackTrace(); 


println("user " + userName + " saved"); 


User Recognition 


Now you have all the data you need from your current user, and you want to know if the same user was 
previously registered in the system. This means that there is a CSV file containing very similar limb 
lengths to the ones of the current user. 

In order to find this out, you make use of the generalization of the concept of vector subtraction to a 
higher-dimensional space. What? Well, the next few lines are a technical explanation of how to measure 
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the similitude between two users based on the skeleton data. If you are not that interested in 
multidimensional vector operations, simply skip the math and use the code provided next! 


Vector Subtraction in Higher Dimensions 


You are using the Cartesian coordinate system with basis vectors. 


e, =(1,0,0) e, =(0,1,0) e, =(0,0,1) 


If the subtraction of two two-dimensional vectors is 


a-b=(a,-b,)e,+(a,-b,)e, 


and the subtraction of two three-dimensional vectors is 


a-b=(a,-b)e, +(a,-b,)e, + (a, - bye, 


then the subtraction of n-dimensional vectors will be 


a-b=(a,-b,)e,+(a, -b,)e, t......+ (a, -,)e, 


This means that you can treat your user data array as a multidimensional vector and calculate 
the distance or difference between two users as the magnitude of the vector resulting from subtracting 


the two. 


a ae - 2 2 2 2 
a-b=c lcl-Ver tte +.+e 


This is performed by the function getUserDistance(), which takes an array as parameter and returns 
the magnitude of the vector resulting from the subtraction of the vector “pattern” and the one formed 
by the user data, excluding the two first parameters, so the recognition doesn’t get affected by changes in 


weight or height. 
float getUserDistance(float[] pattern) { 


float result = 0; 
for (int i = 0; i < pattern.length; i++) { 


result += pow(pattern[i] - userData[i + 2], 2); 


return sqrt(result); 


Enrique 13/11/1982 

29/11/2011 63.0 1686.2014 268.18143 227.60263 263.1212 236.44228 279.46335 305.47165 396.49283 270.1577 393.8149 
Ciriaco 06/12/1979 | | | | | | | | | 
30/11/2011 59.7 1828.7333 288.81677 236.1468  285.55222 241.69952 297.28445 402.2311 419.12512 366.3181 419.0992 


Figure 10-16. Users being compared to previously stored information 
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The function userRecognition() is the bit of code that deals with comparing the current user to the 
information previously stored in CSV files (Figure 10-16). First, you create an array of floats that stores 
the distances of the current user to the various previously stored users, hence the size of it being 
files.length. 

Then, for each user file, you read its content; if the line contains more than two chunks of data (so 
it’s not the header), you store all the relevant floats in the array pattern, and you get its distance to your 
current user data using the previously defined function getUserDistance(). 


private void userRecognition() { 
float[] distances = new float[files.length]; 
for (int i = 0; i< files.length; i++) { 
String line; 
try { 
BufferedReader reader = createReader(files[i]); 
while ( (line = reader.readLine ()) != null) { 
String[] myData = line.split(","); 
if (myData.length > 2) { 
float[] pattern = new float[9]; 
for (int j = 3; j < myData.length; j++) { 
pattern[j - 3] = Float.valueOf(myData[j]); 


distances[i] = getUserDistance(pattern) ; 


} 


catch (IOException e) { 
e.printStackTrace(); 
println("error"); 


Once you have all the distances, or differences, you only need to find the user that is closest to your 
current user; if this distance is small enough (after some testing, we set the minimum to 100), you set the 


user in the file as the current user (Figure 10-17). 


int index = 0; 
float minDist = 1000000000; 
for (int j = 0; j < distances.length; j++) { 
if (distances[j] < minDist) { 
index = j; 
minDist = distances[j]; 


} 


if (minDist < 100) 
setPreviousUser(files[index]); 
println("Recognising User"); 


The function setPreviousUser() simply retrieves the user data from the selected file and updates 


the user name and birth date. 


private void setPreviousUser(File file) { 


String line; 
try { 
BufferedReader reader = createReader(file); 
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line = reader.readLine(); 

String[] myData = line.split(","); 
userName = myData[0]; 
userBirthDate = myData[1]; 
getDate(); 


} 

catch (I0Exception e) { 
e.printStackTrace(); 
println("error"); 


println("User set to " + "userName, born on" + userBirthDate) ; 


Figure 10-17. Users being compared 


Reading and Drawing the Chart 


The following functions follow the same principles and methods you used to read the user data from 
external files, but this time you read the chart files and transform the numeric data into graphs 
representing the percentiles of BMI. 


private void readChart(File file) { 
String line; 
int lineNumber = 0; 


try { 
int lines = count(file); 
chart = new float[lines][11]; 
BufferedReader reader = createReader(file); 
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while ( (line = reader.readLine ()) != null) { 


String[] myData = line.split(","); 
if (lineNumber == 0) { 


else { 
for (int i = 0; i < myData.length; i++) { 
chart[lineNumber - 1][i] = Float.valueOf(myData[i]); 
} 


lineNumber++; 


} 


! 

catch (IOException e) { 
e.printStackTrace(); 
println("error"); 


The function count () is a helper function used to find out how many lines you will be reading from a 
CSV file. 


public int count(File file) { 
BufferedReader reader = createReader(file); 
String line; 
int count = 0; 
try { 
while ( (line = reader.readLine ()) != null) { 
count++3 


} 

catch (I0Exception e) { 
e.printStackTrace(); 
println("error"); 


return count; 


The function drawChart() simply runs through the chart array and draws the lines defining the 
percentiles. You can apply this method to every chart you have included in the data folder of the sketch. 


private void drawChart() { 
float scaleX = 2; 
float scaleY = 10; 


pushStyle(); 
stroke(255); 
noFill(); 
pushMatrix(); 
translate(0, height); 
scale(1, -1); 
translate(660, 20); 
line(0, 0, 500, 0); 
line(0, 0, 0, 300); 
for (int i = 1; i < chart[o].length; i++) { 
beginShape(); 
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for (int j = 0; j < chart.length - 1; j++) { 
vertex(chart[j][0] * scaleX, chart[j][i] * scaleY); 


endShape() ; 


strokeWeight (10) ; 
stroke(255, 0, 0); 
point (userAge*12*scaleX, userBMI*scaleY) ; 
popMatrix(); 
popStyle(); 


Graphic Output 


The following functions handle the on-screen representation of the user data in text form and the 
drawing of the user’s skeleton. 


private void printUserData() { 
£i11(255); 


text("Data at "+ today, 660, 20); 


text("Current User: 


+ userName, 660, 40); 


text("Date of Birth " + userBirthDate, 660, 60); 
text("Current Age " + userAge, 660, 80); 


text("User Weight: " + userData[o] + " " + lcd.units, 660, 100); 
text("User Height: " + userHeightTemp/1000 + " m", 660, 120); 


text("User BMI: " 


£111(255, 255, 150); 
text("BMI at ages 2-20", 1000, 20); 


} 


void drawSkeleton(int userId) { 
pushStyle(); 
stroke(255, 0, 0); 
strokeWeight (3); 


kinect. 


kinect. 


kinect 


kinect 


kinect 


kinect. 


kinect 


kinect 


drawLimb(userId, 


drawLimb(userId, 


.drawLimb(userId, 
kinect. 


drawLimb(userId, 


.drawLimb(userId, 
kinect. 


drawLimb(userId, 


.drawLimb(userId, 


drawLimb(userId, 


.drawLimb(userId, 
kinect. 
kinect. 
kinect. 


drawLimb(userId, 
drawLimb(userId, 
drawLimb(userId, 


.drawLimb(userId, 


SimpleOpenNI. 


SimpleOpenNI. 
SimpleOpenNI. 
SimpleOpenNI. 


SimpleOpenNI. 
SimpleOpenNI. 
SimpleOpenNI. 


SimpleOpenNI. 
SimpleOpenNI. 
SimpleOpenNI. 
SimpleOpenNI. 
SimpleOpenNI. 


SimpleOpenNI. 


+ userBMI + " kg/m2", 660, 140); 


SKEL_HEAD, SimpleOpenNI.SKEL_NECK); 


SKEL_NECK, SimpleOpenNI.SKEL_LEFT_SHOULDER) ; 
SKEL_LEFT SHOULDER, SimpleOpenNI.SKEL_LEFT ELBOW); 
SKEL_LEFT ELBOW, SimpleOpenNI.SKEL_LEFT HAND); 


SKEL_NECK, SimpleOpenNI.SKEL_RIGHT_ SHOULDER) ; 
SKEL_RIGHT SHOULDER, SimpleOpenNI.SKEL_RIGHT ELBOW) ; 
SKEL_RIGHT ELBOW, SimpleOpenNI.SKEL_RIGHT HAND) ; 


SKEL_LEFT SHOULDER, SimpleOpenNI.SKEL_TORSO) ; 
SKEL_RIGHT SHOULDER, SimpleOpenNI.SKEL_ TORSO); 
SKEL_TORSO, SimpleOpenNI.SKEL_LEFT HIP); 
SKEL_LEFT HIP, SimpleOpenNI.SKEL_LEFT KNEE); 
SKEL_LEFT KNEE, SimpleOpenNI.SKEL_LEFT FOOT); 


SKEL_TORSO, SimpleOpenNI.SKEL_RIGHT HIP); 
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kinect.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT HIP, SimpleOpenNI.SKEL_RIGHT KNEE); 
kinect.drawLimb(userId, SimpleOpenNI.SKEL_ RIGHT KNEE, SimpleOpenNI.SKEL_RIGHT FOOT); 


popStyle(); 
} 


Serial Event 


The serialEvent() function is entirely derived from the previous example implemented in the “Using 
the Weight Data” section. This function acquires the data from Arduino and sets the LCD state to the 
incoming data. 


public void serialEvent(Serial myPort) { 
// get the ASCII string: 
String inString = myPort.readString(); 
inString = inString.substring(0, inString.indexOf('\n') - 1); 
if (inString != null) { 
if (inString.equals("S")) { 
currentLine = 0; 


} 

else if (inString.charAt(0) == 'X') { 
String newState = inString.substring(1); 
lcd. setState(Integer.valueOf(newState) ) ; 


: 
else { 
if (inString.length() == 13 && currentLine < 4) { 
for (int i = 0; i < stringData[o].length; i++) { 
stringData[currentLine][i] = inString.charAt(i); 


currentLine++; 
} 
} 


else { 
println("No data to display"); 


Simple-OpenNl User Events 


These are the same good old Simple-OpenNI callbacks as usual. The only hint of novelty is the last line in 
the onNewUser() function, which increases your Processing frame height by 54 pixels so the user interface 
is displayed. 


public void onNewUser(int userId) { 
println("onNewUser - userId: " + userId); 
println(" start pose detection"); 
kinect.startPoseDetection("Psi", userId); 
frame.setSize(width, height + 54); 


} 


public void onLostUser(int userId) { 
println("onLostUser - userId: " + userId); 
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} 


public void onStartCalibration(int userId) { 
println("onStartCalibration - userId: " + userId); 


public void onEndCalibration(int userId, boolean successfull) { 
println("onEndCalibration - userId: " + userId + ", successfull: 
+ successfull); 


if (successfull) { 
println(" User calibrated !!!"); 
kinect.startTrackingSkeleton(userId); 
user = userId; 


else { 
println(" Failed to calibrate user !!!"); 
println(" Start pose detection"); 
kinect.startPoseDetection("Psi", userId); 
} 
} 


public void onStartPose(String pose, int userId) { 
println("onStartdPose - userId: " + userId + ", pose: " + pose); 
println(" stop pose detection"); kinect.stopPoseDetection(userId) ; 
kinect.requestCalibrationSkeleton(userId, true); 


public void onEndPose(String pose, int userId) { 
println("onEndPose - userId: " + userId + ", pose: " + pose); 


Summary 


This has been quite a heavy project on the programming side. You had to acquire rather noisy data from 
a bathroom scale’s LCD, and you made sense of it by reverse-engineering its logic until you had the 
numeric data you wanted. You then built a whole framework for human biometric scanning and you 
implemented a user interface plus a data storage and retrieval system. You even managed to perform 
user recognition using NITE’s skeleton tracking capabilities, and you output all the gathered data on 
screen for visualization. 

You used some Kinect biometrics in this project; obviously, there are innumerable spin-off 
projects to be generated from this. You could start by building a system to show all the available 
growth and weight charts on screen, or you could extract only the user-recognition implementation 
and build your own home security system. You could also implement a fitness evaluator from the user 
shape, and try to classify people according to body shapes and dimensions. If you have some 
commercial vision, you should definitely give a go to an online fitted clothes try-on service based on 
Kinect’s three-dimensional scanning! 
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Figure 10-18. Leonardo’s Vitruvian Man, and a not-so-Vitruvian Kinect user 
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3D Modeling Interface 


by Enrique Ramos 


So far you've used Kinect to accurately track your movements in space, but when it comes to “clicking,” 
you can’t seem to dispense with your mouse. In this project, you’re going to build a wearable user 
interface that will complement Kinect’s tracking capabilities with accurate multi-clicking and you will 
use it to build 3D geometries by drawing directly in space. 

Your user interface will be mounted on a standard glove and will use an Arduino LilyPad as its 
computational core. You will learn how to use the bending of your fingers as an input device via flex 
sensors. You will then learn how to develop your own simple CAD program to translate the position of 
your hand and the bending of your fingers into 3D geometries (as shown in Figure 11-1) that you will 
then export as .dxf files readable by other CAD and 3D-modeling packages. 


? 


Figure 11-1. 3D modeling with the glove interface 
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The Interface 


In previous chapters, you explored several ways of interacting with your computer using Kinect’s 3D 
scanning plus NITE’s hand and skeleton tracking capabilities. But the “clicking” precision using gesture 
recognition is rather inaccurate and can be discouraging for some applications. 

In this chapter, you are going to bridge this gap by building a wearable wireless user interface that 
can be used as a 3D interface for several applications (Figure 11-2). You will take advantage of Kinect’s 
excellent 3D hand-tracking capability, and you will complement it with a way to trigger different 
behaviors in your program without using conventional keyboard and mouse devices. 


Figure 11-2. 3D modeling interface 


Table 11-2 lists all the components necessary for this project and Figure 11-3 shows what they look like. If 
you need to buy all of the Arduino LilyPad stuff, we recommend the LilyPad Beginner’s Kit from SparkFun, 
which includes most of the components for this project plus other parts you may find useful later. 


Table 11-1. Parts List 


Part Description Price 

One glove (I guess you will have to buy two though...) From $5 

4 flex sensors FLX-01-M from Images SI or similar $10.00 each 
4 10KQ resistors For the voltage dividers $0.25 each 
Arduino LilyPad Simple You could also use the LilyPad main board $19.95 
LilyPad XBee XBee breakout for LilyPad $14.95 
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Part Description Price 

XBee Explorer USB You will need a mini USB cable to connect it. $24.95 

2 XBee modules XBee 1mW Chip Antenna - Series 1 $22.95 each 
FTDI Basic Breakout 5V You will need this to program your LilyPad. $14.95 

USB A to MiniB Cable To connect the FTDI to the computer $4.95 
Conductive thread You only need a 60ft bobbin for the project. $2.95 
Needle To sew your own circuit $1.00 

LiPo Battery - 110mAh Polymer lithium ion battery $6.95 


Note You are using the Arduino LilyPad Simple in this project. As a result, you also need to use Arduino’s 
SofwareSerial library to communicate with the XBee radio module, as you can’t access pins 0 and 1 on the LilyPad 
Simple. This way of transmitting information is actually much slower than using pins 0 and 1, but it’s good enough 
for this project. If you are planning on adding more demanding transmission of information to this project, you 
should consider using the standard Arduino LilyPad board. For more information, see the “Going Wireless” section. 


Figure 11-3. Project parts 
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Arduino LilyPad 


When working on wearable technology with Arduino, it’s difficult to find a better option than the 
Arduino LilyPad, an Arduino board developed cooperatively by Leah Buechley and SparkFun Electronics 
specifically to be sewn to fabric and clothing for wearable applications. 

For this project, you are going to use the Arduino LilyPad Simple board. This board has fewer 
connections than the standard Arduino LilyPad, but it includes a built-in power socket for plugging in a 
LiPo battery plus an on/off switch. This will simplify the circuit by eliminating the need to sew an extra 
power supply to your glove. 

Note that there’s no USB connection on the LilyPad board (Figure 11-4). To connect the board to 
the computer, you will need to use an FTDI Basic Breakout that plugs into the 6-pin male header on the 
top of the LilyPad board. The FTDI Basic Breakout (Figure 11-5) can be plugged to the computer with a 
USB MiniB cable. 


Figure 11-5. FTDI Basic Breakout 
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Flex Sensors 


Flex sensors are electronic components that change resistance when bent. As with any other kind of 
variable resistors, they must be used with a voltage divider to get changing voltages into your analog 
input pins (Figure 11-6). This kind of sensors has been used extensively in the fabrication of virtual 
reality gloves. 


5V 
10K Resistor 
Analog Input Pin 
Flex Sensor 


GND 
Figure 11-6. Voltage divider for flex sensor 


For this project, you will use four two-directional Bi-Flex Sensors from Images Scientific 
Instruments. These sensors decrease their resistance as they are bent in either direction. (This feature is 
something you don’t really need for the project, as your fingers only bend in one direction, so you can 
choose any other flex sensors as long as they fit nicely into the glove’s fingers.) 

There are three versions of the FLX-01 sensors, depending on the resistance ranges. This project 
calls for the FLX-01-M (medium resistance range of 20K-50K) and 10K resistors for each sensor, which 
provides a reasonable input range. The kind of circuit you are using for each sensor is called a voltage 
divider (see Figure 11-6). We discussed the calculation of output voltages and resistors for voltage 
dividers in the section “Feedback from Arduino” in Chapter 4. Go back to this section if you need a 
refresher. 

The first thing you need to do is to install the flex sensors in the glove. There are many ways of 
doing this. Our gloves had a double skin, so we decided to insert the sensors between the fake-leather 
exterior skin and the fluffy interior one. After measuring the length of the sensor from the tip of each 
finger, cut some small incisions, just large enough for the sensors to slip into. Then insert one sensor in 
each finger, leaving the cables to protrude from the slits. Only use sensors for the thumb, index, middle, 
and ring fingers (Figure 11-7). Leave the little finger out because of the difficulty of controlling it 
independently (unless you are a piano player!) and because there are only four accessible analog pins 
on the LilyPad Simple. 
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Figure 11-7. Flex sensors installed in the glove 


Sewing the Circuit 


Yes, that’s the right word. This time you won’t be “building” your circuit; you’ll be “sewing” it (Figure 
11-8). For wearable applications, you use conductive thread instead of normal wires to connect the 
components in the circuit; the result is a more flexible, stable, and lightweight design. A piece of advice: 
if you have never sewn a button in your life, you may want to get some help from more experienced 
relatives or friends before you put a needle in your hand! 


Figure 11-8. Sewing the circuit 
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The main idea is to link the LilyPad pins to the right components with the conductive thread. Note 
that you want to avoid the threads touching each other, just as you would do on a normal circuit. 
Sometimes you will need to cross threads but you don’t want them to join electrically. The best way 
around this is to arrange for them to be on different sides of the skin at the crossing point. 

Start by sewing a test circuit using your Arduino LilyPad and the four flex sensors. After proper 
testing, you can add other components. 

First, sew the Arduino LilyPad to the glove, and then sew the connections to the flex sensors and the 
resistors (Figure 11-9). To connect the flex sensors, expose a section of the cables and loop around the 
exposed wires with your conductive thread. Then cut off the rest of the cable. If you look carefully at 
Figure 11-10, you can see that the ground thread connecting all the resistors together goes under the 
exterior skin most of the time so that it doesn’t connect to the cables going from the flex sensors to the 
Arduino LilyPad pins. 


Figure 11-9. Flex sensors circuit 
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Figure 11-10. The circuit sewn to the glove 


Test that the connections are correct using a multimeter and then connect the board to your 
computer using the FTDI Basic Breakout. Now you can load and test your first program (Figure 11-11). 
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Figure 11-11. The LilyPad connected to the FTDI Basic Breakout 
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Testing the Circuit 


Now that you have sewn the circuit, you need to check that everything works properly. To do so, you will 
upload an Arduino program to your board to read the input from the flex sensors and send it via serial so 
you can see the values on the screen. 


Arduino Serial Sender 


Open a new Arduino sketch and declare four integer variables, one for each finger. In the setup() 
function, start serial communication at 9600 baud. 


int finger1, finger2, finger3, finger4; 
void setup() { 
Serial. begin(9600) ; 


In the loop() function, read the four sensors using analogRead() and send their values to the serial 
buffer, separating them by blank spaces and finishing with a print1n() function call to get a newline 
character at the end of each cycle. 


void loop() { 


finger1 = analogRead(A5); 
finger2 = analogRead(A4); 
finger3 = analogRead(A3); 
finger4 = analogRead(A2); 


Serial. print (finger1) ; 
Serial.print(" "); 
Serial. print (finger2) ; 
Serial.print(" "); 
Serial. print (finger3) ; 
Serial.print(" "); 
Serial. print1n(finger4) ; 


Upload this code and open the Serial Monitor. If you put the glove on and bend your fingers, you 
should see the numbers changing within a range of 100 to 700 or similar. 


Processing Glove Data Receiver 


It will be much easier to understand this data if you can get a visual representation of it, so you’re going 
to build a simple Processing sketch that will receive the values and draw them on screen as variable- 
width rectangles. You will reuse some of these elements in the final project. 

Open a new Processing sketch and import the Serial library. Then initialize the serial port and an 
array of four floats to contain the data from the flex sensor for each finger. The data from Arduino is 
actually integers, but you declare the fingerData array as a float data type because you are smoothing 
out the incoming data and the result may turn out to be floating point numbers. Then, within the 
setup() function, set the sketch size and initialize the serial communication. 
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import processing.serial.*; 
Serial myPort; 
float[] fingerData = new float[4]; 


void setup() { 
size(800, 300); 
// Serial Communication 
String portName = Serial.list()[0]; 
myPort = new Serial(this, portName, 9600); 
myPort.bufferUntil('\n'); 


The draw() function begins by defining a black color for the background and then drawing a 
rectangle for each finger, the width based on the finger data from Arduino. 


void draw() { 
background(0) ; 
for (int i = 0; i < fingerData.length; i++) { 
rect(0, 50+i*50, fingerData[i], 20); 
} 
I 


The serialEvent() function is triggered every time you get a newline character in the serial buffer. 
You have programmed the Arduino to send the four values from the flex sensors separated by blank 
spaces, so you need to get the string and split it at every blank space. 


public void serialEvent(Serial myPort) { 
String inString = myPort.readStringUntil('\n'); 
if (inString != null) { 
String[] fingerStrings = inString.split(" "); 


If you have the correct amount of String values (four), you convert every String to a float and then 
you add it to the fingerData[ ] array. You place the conversion inside a try{} .. catch{} block to avoid 
the program stopping completely when it gets an anomalous character from Arduino, which happens 
occasionally. Use the Processing function lerp() to smooth out quick oscillations in the incoming data. 
Figure 11-12 shows what the data could look like. 


if (fingerStrings.length==4) { 
for (int i = 0; i < fingerData.length; i++) { 
try { 
float intVal = Float.valueOf(fingerStrings[i]); 
fingerData[i] = lerp(fingerData[i], intVal, 0.5); 


catch (Exception e) { 
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Figure 11-12. Finger-bending test 


Going Wireless 


Now that you have the basics of the glove interface working, you are going to improve your device by 
eliminating the cables. The ultimate goal of this project is an intuitive interface that works via your hand 
movements, so having a cable attached to it is not quite ideal. 

To free yourself from the inconvenient cables, you need to add a power source to your glove design 
and an XBee radio module to transmit serial data wirelessly. You have used the XBee radio module in 
several other projects, so you should be acquainted with it already. If you skipped the previous chapters, 
read the introduction to XBee in Chapter 7. 


LilyPad XBee 


Happily, there’s a LilyPad XBee breakout board that works on the same principles as the main LilyPad 
board. You are going to add this XBee breakout to your glove, and you will use an XBee Explorer USB 
connected to your computer to receive the information (Figure 11-13). 


Figure 11-13. XBee Explorer USB 
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Connecting the XBee breakout to your LilyPad board isn’t difficult. The way you normally do it is by 
attaching the + and - pins on the XBee breakout to 5V and ground on the LilyPad and the rx and tx to 
pins 1 and 0. But that’s on a standard LilyPad board. You're using a LilyPad Simple, and after close 
inspection, you see that pins 0 and 1 don’t exist. (Well, they do exist but the architecture of the LilyPad 
doesn’t expose them.) So what should you do? Can you use the XBee module with the LilyPad Simple? 
The answer to this problem is one of the Arduino’s core libraries: SoftwareSerial. 


SoftwareSerial Library 


Arduino’s built-in support for serial communication in pins 0 and 1 occurs via a piece of hardware called 
UART (Universal Asynchronous Receiver/Transmitter), which is built into the ATmega microcontroller. 

You can’t access pins 0 and 1 on the LilyPad Simple, but you can replicate the serial reception and 
transmission functionality on other pins using the SoftwareSerial library, which is included with the 
Arduino IDE. This way of transmitting information is actually much slower than using pins 0 and 1, but 
it’s still good enough to transmit the information from the glove in this project. If you are planning on 
adding some more demanding transmission of information to this project, you should definitely 
consider using the standard Arduino LilyPad board. 


Wireless Arduino Code 


Let’s modify the previous Arduino code to work with the XBee through pins 5 and 6 using this library. 
First, you need to include the SoftwareSerial library and initialize a SoftwareSerial object, passing pin 
numbers 5 and 6 as parameters. Call the object XBee. This is the first time you’re using an Arduino library 
in this book. You have previously included libraries in Processing by selecting Import Library in the 
Sketch menu or typing something like import SimpleOpenNI.*; directly in the text editor. In Arduino, the 
syntax is different because the language is based on C instead of Java. You can still go to Sketch > Import 
Library and choose SoftwareSerial; you'll see a new line of code in your text editor, #include 
<SoftwareSerial.h>, which is the Arduino equivalent of the Processing import statement. 

So wherever you wrote Serial on your previous code, you now write XBee. The resulting code is the 
following: 


#include <SoftwareSerial.h> 
SoftwareSerial XBee(5,6); 
int finger1, finger2, finger3, finger4; 


void setup() { 
XBee. begin(9600) ; 


void loop() { 


finger1 = analogRead(A5); 
finger2 = analogRead(A4); 
finger3 = analogRead(A3); 
finger4 = analogRead(A2); 


XBee. print (finger1) ; 
XBee.print(" "); 
XBee. print (finger2) ; 
XBee.print(" "); 
XBee. print (finger3) ; 
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XBee.print(" "); 
XBee. print1n(finger4) ; 


This is the Arduino code that you will be using for the rest of this chapter. Before trying this code 
with the previous Glove Receiver Processing sketch, you need to sew the XBee to the glove circuit. Figure 
11-14 shows the final circuit for the project with the components arranged as in the physical device. It 
will take you some time to sew all the connections. Be sure to make the connections solid and reliable; 
otherwise, the movement of your hands will end up loosening them. 

Apart from the XBee, you need to introduce the LiPo battery in order to make the glove completely 
autonomous. Just plug it in the LilyPad power supply socket and put the battery inside the glove (Figure 
11-15). If your gloves have a double skin, this is the perfect place to hide it. 
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Figure 11-14. Glove circuit completed 


Figure 11-15. The LilyPad Xbee on the circuit (left) and the LiPo battery (right) 
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Implementing the Modeling Interface 


Now that the physical device is ready and the code is uploaded to the Arduino LilyPad and sending serial 
data, it’s time to write your 3D modeling interface program in Processing. 

Let’s define 3D modeling as the creation of 3D geometries with specialized software. These 
geometries are defined by surfaces and lines, which in turn are defined by points. You begin by defining 
points in space and then linking these points together with lines and surfaces, as you can see in Figure 
11-16. You implement this by writing a very simple computer-aided design (CAD) software program 
(detailed in the “Geometric Classes” section) that you can control with your glove. This software is based 
on a main Processing routine and three helper classes that deal with particular functions and data 
structures. 

The first class, GloveInterface, is derived from your previous Glove Data Receiver example and 
deals with the information coming from the glove via serial. The other three classes are the Point, Line, 
and Shape classes, which are the basic blocks of your geometrical constructions. 


Figure 11-16. 3D modelling with the glove interface 


The GlovelInterface Class 


This class deals with the finger-bending information from the flex sensors. Due to the different sizes and 
forms of your fingers and the particularities of the different flex sensors, the ranges of the data coming 
from each one of the fingers will be slightly different. You want to control the incoming data with the 
same functions, so you need to map these values to standard ranges with a calibration function. 
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First, declare the GloveInterface class in its own tab, as you saw in Chapter 7. Then declare a File 
object to store the calibration information so you don’t need to repeat the calibration process every time 
you run the program. 


public class GloveInterface { 
File fingerCal; 


You need three PVectors to handle the position of your hand in space, the position that you consider 
to be the origin of coordinates (which is determined by the position of the hand at the moment of 
recognition), and the position of the glove in your model space (pos). 


PVector pos = new PVector(); 
PVector zeroPos = new PVector(); 
PVector handPos = new PVector(); 


You also need an array to store the bending state of each finger as per the data from the Arduino 
(fingerState[ ]) and then after calibration (fingerStateCal[ ]). The calibration data is stored in the 
fingerCalibration[] nested array, which you initialize to predetermined values. 

The integer currentFinger defines the finger that is “clicked” at any specific moment, which is 
determined using the setFingerValues() function. The default value (99) doesn’t correspond to any of 
the four fingers you are following and it’s identified as no finger clicked. 


float[] fingerState = new float[4]; 

float[] fingerStateCal = new float[4]; 

float[][] fingerCalibration = {{0,600}, {0,600}, {0,600}, {0,600}}; 
public int currentFinger = 99; 


You only implement one constructor taking no arguments. Within this constructor, specify the file 
containing your calibration data. If the file already exists, get the data in the file using the function 
getCalibration(). 


GloveInterface() { 
fingerCal = new File("data/fingerCal.txt"); 
if (fingerCal.exists()) { 
getCalibration(); 


} 


Setter Functions 


You implement three setter functions: one to set the fingers’ state, another to set the current glove 
position, and a third one to set the initial or zero position. 

The function setFingerValues() is called from the main routine every time you get new data from 
Arduino via the serial interface. This function takes an array of strings as its first argument. The first step 
in the function is to convert each String in the array into a float and store them in the fingerState| ] 
array. This code is very similar to the code from the Glove Data Receiver sketch, but it uses a try{} .. 
catch{} block to avoid the execution, stopping when incorrectly formatted strings are received. 


public void setFingerValues(String[] fingerStrings) { 
for (int i = 0; i < fingerState.length; i++) { 


try { 
float intVal = Float.valueOf(fingerStrings[i]); 
fingerState[i] = PApplet.lerp(fingerState[i], intVal, 0.2f); 


catch (Exception e) { 
} 
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After acquiring the fingers’ states, use the fingerCalibration values to remap the incoming data 
from the calibration values to the desired range of 0-600. The way you calibrate the interface is by 
opening all your fingers and storing the “stretched” values into the zero index element of each array in 
fingerCalibration. Then you close your fingers and store the “maximum bending” state into the index 1 
of each array in fingerCalibration. By mapping each of the incoming values from these ranges to the 
desired range (0-600), you make sure that a stretched finger means a value close to 0 and a fully bent 
finger casts a value close to 600. You analyze these calibration functions at the end of the class. 


for (int i = 0; i < fingerStateCal.length; i++) { 
fingerStateCal[i] = PApplet.map(fingerState[i], fingerCalibration[i][o], 
fingerCalibration[i][1], 0, 600); 


Finally, you determine if one of the fingers is “clicked” by checking which one of the fingers presents 
a higher bending state and if its value is over a threshold (300). The current finger is stored in the integer 
currentFinger. 


float maxBending = 0; 
currentFinger = 5; 
for (int i = 0; i < fingerStateCal.length; i++) { 
if (fingerStateCal[i] > maxBending && fingerStateCal[i] > 300) { 
maxBending = fingerStateCal[i]; 
currentFinger = i; 
} 
} 
} 


The function setPosition() takes a PVector defining the current position of your hand in global 
coordinates, which is stored in the handPos PVector. Then you translate this value to your modeling 
coordinate system by subtracting the zeroPos PVector from it. The zeroPos PVector updates from the 
main routine by calling the setZeroPos() function. 


public void setPosition(PVector inputPos) { 
this.handPos = inputPos; 
this.pos = PVector.sub(inputPos, zeroPos); 


} 


public void setZeroPos(PVector handPos) { 
this.zeroPos = handPos; 


} 


Display Functions 


The draw() function simply draws a point on screen of the coordinates of your glove interface. The 
switch() statement changes the color of the point depending on the current finger. 


public void draw() { 
pushStyle(); 
switch (currentFinger) { 
case 0: 
stroke(255, 0, 0); 
break; 
case 1: 
stroke(0, 255, 0); 
break; 
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case 2: 
stroke(0, 0, 255); 
break; 

case 3: 
stroke(255, 255, 0); 
break; 

case 5: 
stroke(255, 255, 255); 
break; 


} 
strokeWeight (20); 
if(!record){ 

point(pos.x, pos.y, pos.z); 


popStyle(); 


Note that you put the point() function within an if() statement that only draws if the Boolean 
record is not true. This is due to the dxf export library that you will be bringing in later; this Processing 
library can’t export points so you need to avoid the printing of points when exporting. You do this every 
time you draw points in this program. 

The drawData() function draws the same information on screen as the Glove Data Receiver sketch, 
letting you see the calibrated bending state of each finger. 


public void drawData() { 
for (int i = 0; i < fingerStateCal.length; i++) { 
rect(0, 50 + i * 50, (int) fingerStateCal[i], 20); 


} 
Calibrating the Interface 


We have already explained how the calibration of the device works. The following three functions take 
care of setting the calibration data, storing it in the external file, and retrieving it on demand. 

The function calibrateFinger() is called from the main Processing routine by pressing a key on the 
keyboard. It takes the finger to be calibrated (i) and the state being calibrated (j), which is 0 if calibrating 
the stretched state or 1 if calibrating the bent state. You first store the present non-calibrated state of the 
finger into the fingerCalibration[ ] array and then call the saveCalibration() function. 


public void calibrateFinger(int i, int j) { 
fingerCalibration[i][j] = fingerState[i]; 
saveCalibration(); 


} 


saveCalibration() converts the fingerCalibration double array into a linear array of strings and 
uses the saveStrings() function to store all the values into the fingerCal file. 


private void saveCalibration() { 
String[] mySettings = new String[8]; 
for (int k = 0; k < fingerCalibration.length; k++) { 
mySettings[k] = String. valueOf(fingerCalibration[k][0]); 
mySettings[k + 4] = String. valueOf(fingerCalibration[k][1]); 


PApplet.saveStrings(fingerCal, mySettings) ; 
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Lastly, the function getCalibration() performs the opposite process, using loadStrings() to 
retrieve the strings from the fingerCal file and storing these values into the fingerCalibration array 
after converting them into floats. 


private void getCalibration() { 
String[] mySettings = PApplet.loadStrings(fingerCal) ; 
for (int k = 0; k < fingerCalibration.length; k++) { 
fingerCalibration[k][0] = Float.valueOf(mySettings[k]); 
fingerCalibration[k][1] = Float.valueOf(mySettings[k + 4]); 
} 
} 
} 


Geometric Classes 


In order for you to be able to draw in three dimensions with your new glove interface, you need to 
implement your own modeling software or link your Processing interface to existing CAD software. 
You're going to take the first approach by implementing a very, very simple and limited CAD program. 
CAD, or computer-aided design, is the software that allows engineers, architects, and designers to 
draft with a computer. There are plenty of commercial CAD and 3-D modeling software packages—and a 
few good open source options. The very basic blocks of the geometrical constructions generated with 
modeling software are the notions of point, line, and surface. You are going to create a separate class for 
each of these basic geometric concepts, which you will then use to generate the geometry (Figure 11-17). 


Figure 11-17. Points, lines, and surfaces defined by your program 


Point Class 


As you build your geometry, a point is defined as a specific location in 3D space. This location is 
described by three coordinates: x, y, and z. For your purposes, this location can be treated as a vector 
(x,y,z), which means you could use the PVector class to define your points. Instead, you are going to 
write your own Point class to be able to add a couple more functions to your point objects. 

Note that the class Point will have two fields: a PVector describing its position and a Boolean 
variable that is true if the element has been selected. You implement only one constructor taking a 
PVector as parameter that is used as the point’s position. 


public class Point { 
PVector pos; 
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private boolean selected; 


Point(PVector pos) { 
this.pos = pos; 


} 


The draw() function displays the point on screen. Note that you again put the point() function 
within an if() statement that only draws if the Boolean record is not true. 


void draw() { 
pushStyle(); 
strokeWeight (10) ; 
if (selected) { 
stroke(255, 255, 0); 


else { 
stroke(200) ; 


if (!record){ 
point(pos.x, pos.y, pos.z); 


popStyle(); 


Finally, implement three more functions that allow you to change the position of the point, select it, 
and unselect it (Figure 11-18). 


public void setPos(PVector newPos) { 
this.pos = newPos; 


} 


public void select() { 
this.selected = true; 


} 
public void unSelect() { 
this.selected = false; 
} 
} 


Figure 11-18. Selected point 
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Line Class 


As you saw in the previous section, a line is a one-dimensional geometrical element. A line is by definition 
infinite, but in this case, whenever we talk about lines, we’re actually referring to line segments. 

A line segment is a portion of a line, and it can be defined by two points. You have probably heard 
line defined as “the shortest path between two points,” which is pretty close to the sense the word line 
will have in your code. 

Your Line class, then, has two Point objects as fields. These two points define the line and are used 
within the draw() function to display the line on screen by using Processing’s line() function. 


public class Line { 


Point p1; 

Point p2; 

Line(Point pi, Point p2) { 
this.p1 = p1; 
this.p2 = p2; 

} 

void draw() { 
pushStyle(); 
stroke(255); 


line(p1.pos.x, p1.pos.y, p1.pos.z, p2.pos.x, p2.pos.y, p2.pos.z); 
popStyle(); 
} 


Shape Class 


The Shape class represents triangular surfaces in your program. A surface is a two-dimensional 
geometrical element. Your Shape objects represent portions of planes, or flat surfaces, defined by three 
points (Figure 11-19). Triangular shapes are the basis of many polygonal modeling programs; for you, 
they constitute the only way in which your software will represent surfaces. 

The Shape class uses an ArrayList of Point elements to store the points defining it. You could have 
used three points instead, but this way lets you extend the concept further and draw complex shapes 
consisting of more than three points. The draw() function makes use of Processing’s beginShape(), 
vertex(), and endShape() functions to draw your shapes on screen. In the future, you might want to 
dynamically add points to this function in runtime, so add a function addPoint() for this purpose. 


public class Shape { 
ArrayList<Point> points = new ArrayList<Point>(); 
Shape( ArrayList<Point> points) { 
for (Point pt : points) { 
this.points.add(pt); 


} 


void draw() { 
pushStyle(); 
£i11(255); 
stroke(150); 
beginShape(); 
for (int i = 0; i < points.size(); i++) { 
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vertex(points.get(i).pos.x, points.get(i).pos.y, points.get(i).pos.z); 
endShape(PConstants.CLOSE) ; 
popstyle(); 


void addPoint(Point newPoint) { 
points.add(newPoint) ; 


Figure 11-19. Freeform three-dimensional geometry built with triangular shapes 


Modeling Interface Main Program 


Now that you have your helper classes implemented, you can write the main routine. This code drives 
the program’s workflow and makes use of the previous classes to achieve the ultimate goal of allowing 
you to make 3D geometries using your glove interface. Add more of Processing’s core libraries so you 
can export the geometries you create to .dxf files and import them into other CAD software. 


Imports and Fields 


You need to import five libraries for this project: three core libraries (OpenGL, Serial, and DXF) and two 
external libraries (Simple-OpenNI and KinectOrbit). 


import processing.opengl.*; 
import processing.serial.*; 
import processing.dxf.*; 
import SimpleOpenNI.*; 
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import kinectOrbit.*; 


KinectOrbit myOrbit; 
SimpleOpenNI kinect; 


Serial myPort; // Initialize the Serial Object 
boolean serial = true; // Define if you're using serial communication 


You are implementing hand recognition using NITE’s XnVSessionManager, as you did in Chapter 5, 
so you must initialize the appropriate objects. 


XnVSessionManager sessionManager; 
XnVPointControl pointControl; 


You then declare the GloveInterface object glove and four ArrayList collections defining the 
geometry’s points, selected points, lines, and shapes. Use ArrayList collections because you don’t know 
how many points, shapes, or lines the user will create; ArrayList collections provide you with the 
flexibility you need for the program. The integer thresholdDist defines how close the glove interface 
needs to be from a point for it to be selected. 


private GloveInterface glove; 

ArrayList<Point> points = new ArrayList<Point>(); 
ArrayList<Point> selPoints = new ArrayList<Point>(); 
ArrayList<Line> lines = new ArrayList<Line>(); 
ArrayList<Shape> shapes = new ArrayList<Shape>(); 
int thresholdDist = 50; 


The boolean record serves to indicate when you want to save your geometry as a .dxf file. It is set to 
false by default, so you avoid creating a new file in the first loop. 


boolean record = false; 


Setup Function 


The setup() function starts by defining the sketch size and renderer, and it then initializes the 
KinectOrbit object. You then set KinectOrbit to draw the orbiting gizmo on screen and the Coordinate 
System axes. You also use the function setCSScale() to set the size of the CS axes to 200 mm. (You can 
change these settings to any other configuration.) 


public void setup() { 
size(800, 600, OPENGL); 
myOrbit = new KinectOrbit(this, 0, "kinect"); 
myOrbit .drawGizmo(true) ; 
myOrbit .drawCS(true) ; 
myOrbit.setCSScale(200) ; 


Now you initialize the Simple-OpenNI object and all the capabilities you need, including the 
sessionManager and pointControl objects. 


// Simple-openni 

kinect = new SimpleOpenNI (this) ; 
kinect.setMirror(false) ; 
kinect.enableDepth(); 
kinect.enableGesture(); 
kinect.enableHands(); 
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sessionManager = kinect.createSessionManager("Wave", "RaiseHand"); // setup NITE 
pointControl = new XnVPointControl(); // Setup NITE's Hand Point Control 
pointControl.RegisterPointCreate(this) ; 

pointControl.RegisterPointDestroy(this) ; 

pointControl.RegisterPointUpdate(this) ; 

sessionManager .AddListener(pointControl) ; 


And finally, you initialize the serial communication and the GloveInterface object. 


if (serial) { 
String portName = Serial.list()[0]; // Get the first port 
myPort = new Serial(this, portName, 9600); 
myPort.bufferUntil('\n'); 

} 


glove = new GloveInterface(); // Initialize the GloveInterface Object 


Draw Function 


As usual, you pack most of the processes into additional functions, so the draw() function presents a 
clear scheme of the program’s workflow. The first step is to update the Simple-OpenNI object (kinect) 
and the sessionManager. Then you set the background to black. 


public void draw() { 
kinect.update(); // Update Kinect data 
kinect .update(sessionManager); // update NITE 
background(0) ; 


This program behaves differently depending on which finger is bent. You do this by means ofa 
switch() statement with four case handlers. The four possible behaviors are moving an existing point, 
creating a new point, adding a line between two points, or adding a new shape (Figure 11-20). 


switch (glove.currentFinger) { 

case 0: 
movePoint(); 
break; 

case 1: 
addPoint(); 
break; 

case 2: 
addLine(); 
break; 

case 3: 
addShape(); 
break; 
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Figure 11-20. Triggering Actions with your Fingers and thumb 


After this switch() statement, all the geometrical operations have been performed, so you only need 
to draw the results on screen. You first push the KinectOrbit object so you can control your point of view 
of the geometry. 


myOrbit.pushOrbit(this); // Start Orbiting 


After pushing the KinectOrbit, check if the boolean record is true; if so, use the beginRaw() method 
to save the geometry into the file output .dxf. Then draw the glove and the points, lines, and shapes. The 
drawClosestPoint() function highlights the closest point to your glove interface if the distance is smaller 
than thresholdDist. 


if (record == true) { 
beginRaw(DXF, "output.dxf"); // Start recording to the file 


glove.draw(); 
drawClosestPoint(); 


for (Point pt : points) { 
pt.draw(); 


for (Line In : lines) { 
In. draw(); 


for (Shape srf : shapes) { 
srf.draw(); 


Before you pop the KinectOrbit, check again the state of the record variable. If it’s true (and you are 
recording the .dxf file), you end the recording and set the record variable to false. After this, you pop the 
KinectOrbit and, if you want to see the finger data printed on screen, call the method drawData() from 
the glove object. 


if (record == true) { 
endRaw(); 
record = false; // Stop recording to the file 


} 
myOrbit.popOrbit(this); // Stop Orbiting 
glove.drawData(); 
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Additional Functions 


The movePoint() function is called when you bend your thumb. It checks all the existing points; if their 
distance to your glove object is under thresholdDist, it changes the point’s position to the position of 
your glove (which is your hand’s position). In this way, you can actually move the points around with 
your hand, just as if they were standing around you in space. 

You use a Java for-each loop to run through the ArrayList points. This is equivalent to using a 
standard for loop, simplifying this common form of iteration. You can read it as “For each Point object 
in the ArrayList points, do { }.” The Point instance pt is set to a different Point object inside points in 
every loop. 


void movePoint() { 
for (Point pt : points) { 
if (glove.pos.dist(pt.pos) < thresholdDist) { 
pt.setPos(glove.pos) ; 


The addPoint() function creates a new point at the position of your glove. Bending your index finger 
triggers this function. To avoid the creation of multiple adjacent points, you won’t create a point if there 
is another point closer than thresholdDist. 


void addPoint() { 
Point tempPt = new Point(glove.pos.get()); 
boolean tooClose = false; 
for (Point pt : points) { 
if (tempPt.pos.dist(pt.pos) < thresholdDist) { 
tooClose = true; 


i 


if (!tooClose) { 
points.add(tempPt) ; 


Bending your middle finger calls the addLine() function. This function proceeds in two steps. First, 
it checks if there is a point closer than the defined threshold. If there is one and it’s not already 
contained in the ArrayList selPoints, it selects the point and adds it to selPoints. 

Then, if there is more than one point selected, it creates a new line between the two points in 
selPoints and adds it to the ArrayList lines. Finally, it unselects everything using the unSelectA11() 
function. 


void addLline() { 
for (Point pt : points) { 
if (glove.pos.dist(pt.pos) < thresholdDist) { 
pt.select(); 
if (!selPoints.contains(pt)) { 
selPoints.add(pt); 
} 


if (selPoints.size() > 1) { 
Line lineTemp = new Line(selPoints.get(0), selPoints.get(1)); 
lines.add(lineTemp) ; 
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unSelectAll(); 


The last geometry creation function, addShape(), is called when you bend your third finger. This 
function works in the same way as addLine(), but it waits until you have three selected points to create a 
new Shape object, adds it to the ArrayList shapes, and unselects all the points. 


private void addShape() { 
for (Point pt : points) { 
if (glove.pos.dist(pt.pos) < thresholdDist) { 
pt.select(); 
if (!selPoints.contains(pt)) { 
selPoints.add(pt); 


} 

if (selPoints.size() > 2) { 
Shape surfTemp = new Shape(selPoints) ; 
shapes.add(surfTemp) ; 
unSelectAll(); 


The unSelectAl1() function runs through all the points in your sketch and unselects them; then it 
clears the selPoints ArrayList. 


void unSelectAll() { 
for (Point pt : points) { 
pt.unSelect(); 


selPoints.clear(); 


} 


The previously used drawClosestPoint() function takes care of changing the onscreen appearance 
of the points that are close enough to the glove to be modified with the other functions. 


void drawClosestPoint() { 
for (Point pt : points) { 
if (glove.pos.dist(pt.pos) < thresholdDist) { 

pushStyle(); 
stroke(0, 150, 200); 
strokeWeight (15); 
point(pt.pos.x, pt.pos.y, pt.pos.z); 
popStyle(); 
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Processing Callbacks 


You use two Processing callback functions in this project. First, the serialEvent() function calls every 
time you receive a newline character from serial. This function acquires the string from the serial 
communication, splits it, and passes the resulting array of strings as a parameter to the glove object 
using the method setFingerValues(), which updates the finger-bending values. 


public void serialEvent(Serial myPort) { 
String inString = myPort.readStringUntil('\n'); 
if (inString != null) { 
String[] fingerStrings = inString.split(" "); 
if (fingerStrings.length == 4) { 
glove.setFingerValues(fingerStrings) ; 


You use the keyPressed() Processing callback function to calibrate your glove interface. Numbers 1 to 
4 set the stretched values for your fingers, and q, w, e, andr characters set the maximum bending values. 
Additionally, the character d triggers the saving of the geometry into the previously defined .dxf file. 


public void keyPressed() { 

switch (key) { 

case '1': 
glove.calibrateFinger(0, 0); 
break; 

case '2': 
glove.calibrateFinger(1, 0); 
break; 

case '3': 
glove.calibrateFinger(2, 0); 
break; 

case '4': 
glove.calibrateFinger(3, 0); 
break; 

case 'q': 
glove.calibrateFinger(0, 1); 
break; 

case ‘w': 
glove.calibrateFinger(1, 1); 
break; 

case 'e': 
glove.calibrateFinger(2, 1); 
break; 

case 'r': 
glove.calibrateFinger(3, 1); 
break; 

case '‘d': 
record = true; 
break; 

} 

} 
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Simple-OpenNl Callbacks 


Finally, you add the Simple-OpenNI callback functions. Within onPointCreate(), you set the zero 
position of your glove. This has the effect of setting the origin of coordinates to the point in space where 
the waving gesture was recognized. This makes it easier for the user to be drawing at an acceptable 
distance from the origin. 


public void onPointCreate(XnVHandPointContext pContext) { 
println("onPointCreate:"); 
PVector handVec = new PVector(pContext.getPtPosition().getX(), pContext 
-getPtPosition().getY(), pContext.getPtPosition().getZ()); 
glove.setZeroPos(handVec.get()); 
} 


public void onPointDestroy(int nID) { 
println("PointDestroy: " + nID); 


Within the onPointUpdate() function, you call the setPosition() method from your glove object to 
update the position of the glove image on screen to reflect your current hand position. 


public void onPointUpdate(XnVHandPointContext pContext) { 
PVector handVec = new PVector(pContext.getPtPosition().getX(), pContext 
-getPtPosition().getY(), pContext.getPtPosition().getZ()); 
glove.setPosition(handVec) ; 


} 


Figure 11-21. 3D geometries created with the interface 


Run this program and start playing with the position of your hands and the bending of your fingers 
(Figure 11-21). Soon you will develop an intuitive understanding of the functioning of the program; 
after some time, you will be able to create some pretty complex 3D geometries without having touched 
your mouse. 

When you are satisfied with your results, press the d key on your keyboard and you'll get a file called 
output. dxf in your Processing sketch folder. Note that .dxf is a file format created by Autodesk (the 
company behind AutoCAD) for importing and exporting data from AutoCAD to other programs. 
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You have chosen .dxf because there is a Processing core library that does all the exporting for you, 
but if you are curious enough to read the next chapter, you will learn how to implement your own 
routine to export .ply files, which are compatible with many other modeling programs. 


Summary 


This project led you through the building of a wearable, lightweight, wireless user interface designed to 
work in combination with Kinect’s hand-tracking capabilities that ultimately allowed you to create 3D 
geometries in an intuitive way, without using any conventional human interface devices. 

You built the interface on a glove, using the Arduino LilyPad board in combination with flex sensors 
to read the bending of your fingers and a couple of XBee radio modules to transmit the information to 
your computer wirelessly. You also learned how to use the Arduino SoftwareSerial library to use any pin 
on the Arduino for serial communication. 

Finally, you implemented your own simplified CAD software to deal with the basic geometrical 
entities and data structures that constituted your constructions. This software is controllable by your 
glove interface. 

This software and interface are just the bare bones of what could be the beginning of a new 
standard in human-computer interfaces. Thanks to the Kinect, you are no longer limited to the two 
dimensions of conventional devices. Now you can use the full mobility of your body as a way to input 
data into your applications. 
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Turntable Scanner 


by Enrique Ramos 


If you're like most people, one of the first things that comes to mind when you see a Kinect point cloud, 
or any 3D scanner point cloud, is that it’s only one-sided. There are a number of techniques that can be 
used to perform a 360-degree volumetric scan and then reconstruct the implicit surface into a three- 
dimensional mesh. One way is to use a turntable. 

In this chapter, you will build a DIY turntable, connect it to your computer, and scan objects from 
every point of view. Then, you will learn how to process the scanned data and patch the different point 
clouds into one consistent point set. There will be a fair bit of trigonometry and vector math involved in 
this process, so you might want to keep a reference book handy. 

You will then implement your own exporting routine to output files containing your point cloud 
data in a format readable by other software. Finally, you will learn how to use Meshlab to reconstruct the 
implicit surface from your point cloud so you can use or modify it in any modeling software, or even 
bring the modified volume back to the physical world with a 3D printer. Figure 12-1 shows the 
assembled system. 


Figure 12-1. Assembled Kinect and turntable 
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The Theory 


The ultimate goal of this project is to be able to scan an object from all 360 degrees around it to get the full 
point cloud defining its geometry. There are a number of techniques that you could use to achieve this. 

First, you could use several Kinect cameras, all connected to the same computer and placed around 
the object you want to scan. There are a couple of downsides to this technique. First, you need to buy 
several Kinect devices, which is expensive. Then, every time you need to scan something you need to set 
up the Kinects, which takes a large amount of space. 

Another option is to use SLAM and RANSAC-based reconstruction. These are advanced 
computational techniques that allow you to reconstruct a scene from different scans performed with a 
3D-scanner without having any kind of information about the actual position of the camera. The 
software analyzes the different point clouds and matches them by detecting the presence of outliers 
(RANSAC stands for RANdom SAmple Consensus, SLAM for Simultaneous Localization and Mapping). 
Nicolas Burrus implemented a very successful scene reconstruction that is included in his software 
RGBdemo (http://nicolas.burrus.name), and Aaron Jeromin has adapted the RGBdemo code to create 
a homemade turntable scanner, the results of which you can see in Figure 12-2 and on its YouTube site 
at http://www. youtube. com/user/ajeromin. 

But turntable scanners are nothing new, nor do they depend on the capabilities of the Kinect. 
Commercial 3D scanning companies offer turntables to be used with their 3D scanning cameras, like 
Mephisto from 4ddynamics. Structured light-based 3D scanners have been around for a while, and you 
can find a few DIY examples on the Internet, such as SpinScan 
(http://www. thingiverse.com/thing: 9972). 


Figure 12-2. Spinscan (left) and an image produced by Aaron Jeromin’s turntable scanner (right) 


As clearly announced in the introduction, you're going to take the turntable approach. But you’re 
not going to rely on any library or complicated algorithms for this project. You are going to build a 
simple but accurate turntable and use very basic geometrical rules to reconstruct a scene from several 
shots of the same geometry taken from different points of view. Then you will use another open source 
piece of software, Meshlab, to reconstruct the unstructured point cloud generated by your Processing 
sketch from the Kinect data. 
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Base Rotation 


Hidden Side 


Figure 12-3. Turntable diagram 


When you scan an object with the Kinect, there is always a visible side, the one that the Kinect can 
“see”, and a hidden side that you can’t capture with a single shot (Figure 12-3). If you mount your object 
on a rotating base, or turntable, you can rotate the object so that point P is on the visible side. You can 
then rotate the turntable to any angle and thus be able to see the object from every point of view. 

But you want to reconstruct the whole object from different shots, so you need to have all points 
defining the object referred to the same coordinate system. In the original shot, point P was defined by 
the coordinates P(x,y,z); when you rotate the platform to see the point, the coordinates of P change, 
being now P(x’,y’,z’). This is only a problem if you don’t know how much the platform has rotated. But if 
you know precisely the angle of the platform from the original shot, you can easily retrieve the original 
coordinates P(x,y,z) from the transformed ones P(x’,y’,z’) using basic trigonometry. Let’s call your angle 
&; you are rotating the point around the y-axis, so the transformation will be the following: 


x= x’ * cos(&) —z’ * sin(a); 
y=y 
zZ=x’ * sin(a) +z’ * cos(Q); 


By applying this simple transformation, you can scan from as many points of view as you want and 
then reconstruct the volume in a consistent coordinate system. You are going to build a turntable that 
will allow you to rotate the object 360 degrees, knowing at all times the precise angle at which you are 
rotating the object so you can retrieve the global coordinates of the scanned points (Figure 12-4). You are 
going to start by building the turntable, and then you will learn how to process the data. 
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VOWS 


Figure 12-4. Reconstruction of an object from three different views. 


This project can be built entirely from cheap parts you can find in hardware stores and hobbyist shops. 
Table 12-1 lists the requirements. 


Table 12-1. The Parts List 


Part Description Price 

2 swivel discs From the hardware store $6.59 each 
2 metal box shapes We used some post bases we found in a DIY shop $7.69 each 
Aluminum angle 2.40m From the hardware store $12.95 
Acrylic sheet Thick enough to be stable $17.59 
Multi-turn potentiometer DFRobot Rotation Sensor V2 $4.74 
Continuous rotation servo Parallax (Futaba) $13.79 
Servo bracket Lynxmotion Aluminum $11.95 
Gears We adapted the gear kit from Vex $12.99 

1 prototype shield SparkFun DEV-07914, but any one will do. $16.95 


Building a Turntable 


A turntable is a platform mounted on a swivel that can rotate around a center point. Yes, it’s exactly like 
the turntable on any record player. We actually considered using an old record player for this project, 
but in the end it was cheaper to build our own. 

There is something that you will need to bear in mind during the whole process of building the 
turntable: accuracy. The purpose of building such a device is to have a platform that can holda 
reasonable weight without collapsing and that can rotate freely using a servo motor. The higher the 
precision of your physical device, the higher the consistency of the point clouds when you patch them 
together, so try to be thorough with your dimensions and tolerances! 
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We started this project by building a prototype of the turntable so we could test basic algorithms 
and principles. The prototype consisted of a cardboard box with a servo sticking out of its top face, as 
you can see in Figure 12-5. On the servo, we attached a circle of cardboard. That was it. This prototype 
was, obviously, not accurate enough: the servo could only rotate 180 degrees so we couldn’t scan our 
object all around. However, it was very useful for understanding the basic principles and for developing 
the software that we will discuss in a later section. 


Figure 12-5. Turntable prototype 


The issue of the 180-degree rotation of standard servos was a key issue in the design of our final 
turntable. We could have used a continuous rotation servo or hacked a standard servo into rotating 
continuously, but then we would lose the feedback. In other words, we could drive the servo forward and 
backward, but we wouldn’t know the precise position of the turntable. An option that immediately came 
to mind was a 360-degree servo. However, power was also an issue for the prototype, so we wanted to 
increase the torque of our motor with gears. The final solution was to use a continuous rotation servo 
and build our own feedback system with a multi-turn potentiometer. The two were connected with 
gears to the turntable, increasing the power of the servo seven times. Let’s proceed step by step to the 
building of the turntable. Figure 12-6 shows all of the parts of the turntable. 
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Figure 12-6. Turntable parts 


Connecting the Gears 


The gears you're using have a one-to-seven ratio, so the large gear rotates once every seven turns of the 
smaller gears. This is perfect for this project because your potentiometer can turn up to ten times, and 
the three gears fit perfectly onto the base. But they come from a gear kit from Vex Robotics, so the gears 
don’t fit naturally with the Parallax servo or the potentiometer. The first thing you need to do is modify 
the gears so you can fit them tightly with the other two components (Figure 12-7). 

For the potentiometer, drill a hole in the middle of the gear slightly smaller than the diameter of the 
potentiometer’s shaft. This way you'll be able to push it in, and it will rotate the shaft without the need 
for gluing or attaching it permanently. Start with a small drill bit and go bigger step by step. Otherwise, 
you might drill a non-centered hole. 

For the motor, you need to drill a hole only about a fourth of the depth of the gear (see Figure 12-7). 
The servo shaft is pretty short, and you’re going to be attaching the gear on the other side of the metal 
base. Don’t worry if the gear is slightly loose; you’re going to secure it to the servo with a screw. 
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Figure 12-7. Gear adapted to the servo 


Now you can attach the potentiometer and the servo to the base. First, check the position of the 
gears so you have an idea of where the holes need to be drilled for the potentiometer (see Figures 12-8 
and 12-9). 


Figure 12-8. Potentiometer asssembled to the base 


315 


CHAPTER 12!) TURNTABLE SCANNER 


Figure 12-9. Gears assembled to the base 


Figure 12-10. Potentiometer and servo assembled to the base 
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The fixing of the servo and the potentiometer is highly dependent on the base element you have 
chosen. In our case, there was enough to space the servo bracket and the potentiometer with a couple of 
nuts. Note that we have attached the potentiometer and the servo to the sides of the base (Figure 12-10) 
instead of the top plate. This is to avoid any protruding bolts or nuts on the top plate that would get in 
the way of the swivel. 

You should achieve a good match for the three gears so they rotate together without effort. The large 
gear will be attached to the acrylic sheet later. Make sure the gears are parallel to the base so there is no 
friction when the motor is in action. 

The next step is to attach the swivel to the base (Figure 12-11), making sure that its center of rotation 
is as close to the center of rotation of the larger gear as you can possibly manage. This is very important 
because a badly centered swivel will not rotate when assembled! 


Figure 12-11. Swivel and gears 


The large gear won’t be attached to the base. You attach it to the acrylic sheet that you previously 
cut in a disc shape 29cm in diameter. You fix the gear to the disc with a central screw, and you add 
another to avoid the differential rotation of the disc and the gear (Figure 12-12). 
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You could declare your turntable ready at this point, but you are going to attach a base for the 
Kinect to it so you don’t have to calibrate the position of the Kinect every time you use the scanner. 

The Kinect base is exactly like the one you used for the turntable, so both will be at the same height, 
and the Kinect will stand at pretty good position. First, cut the aluminum angle into two 1.2m pieces and 
drill three holes on each end to the same spacing of the holes on the bases (Figure 12-13). 


Figure 12-13. Bracing aluminum angles after drilling the holes 
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Figure 12-14. Assembled turntable 


Figure 12-14 shows two bases separated by more than 1m, which is enough for the Kinect to be able 
to scan a medium-sized object. If you are thinking of scanning larger objects, play a little with your 
Kinect first to determine an ideal distance to the turntable. Once you have calibrated your Kinect and 
your turntable (check that the center of your turntable is coincident with the center of the scanning 
volume in your sketch), you can attach the Kinect to the base or simply draw a contour that will tell you 
where to place it every time. If you fancy a cooler finishing, you can countersink the screws and adda 
vinyl record to your turntable as a finishing surface (see Figure 12-15). 


Figure 12-15. Assembled turntable 


Building the Circuit 


The turntable circuit is a very simple circuit with only a servo motor and a potentiometer. You're 
building it on a SparkFun Arduino prototype shield because you want the circuit to be usable for this 
and other projects. The circuit includes an analog reader on pin 0 and a servo, as shown in Figures 12-16 
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and 12-17. The circuit leaves two three-pin breakaway headers ready to for the servo and the 
potentiometer. 


Figure 12-17. Arduino prototype shield (back) 
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The circuit, as depicted in Figure 12-18, connects the three pins of the breakaway headers to 5v, 
ground, and pins analog 0 and digital 6. 
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Figure 12-18. Turntable circuit 


Arduino Code 


You are going to implement an Arduino program that will drive the servo forward and backward 
according to signals coming from your main Processing program and send back the rotation of the 
potentiometer at every point in time. It will also inform the Processing sketch when it reaches the 
intended position. 

The servo is attached to digital pin 6 and the potentiometer to analog pin 0, so create the necessary 
variables for those pins and the pulse for the servo. You also need a couple of long integers for the 
servo timer. 


int servoPin = 6; 

int potPin = 0; 

int servoPulse = 1500; 
long previousMillis = 20; 
long interval = 20; 
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Figure 12-19. Testing the range of the potentiometer 


After some intensive testing sessions, we worked out that 360 degrees on the turntable meant a 
range of 724 in the potentiometer values (Figure 12-19) . So use a start position of 100 and an end 
position of 824. In other words, whenever the potentiometer is at 100, you know your angle is zero; 
whenever the potentiometer reads 824, you know you have reached 360 degrees. The potentiometer 
values change in proportion to the angle, so you can always work out the angle from the values read 
from the potentiometer. 


// Start and end values from potentiometer (0-360 degrees) 
int startPos = 100; 
int endPos = 824; 


The continuous rotation servo doesn’t work like a normal servo. You still have a range of 500-2500 
microseconds for your pulses, but instead of the pulse meaning a specific angle, any pulse under 1500 
microseconds makes the servo to turn forward, and any pulse over 1500 microseconds sends the servo 
turning backward. Create a couple of variables for the forward and backward pulses. 


// Forward and backward pulses 
int forward = 1000; 
int backward = 2000; 
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Your Arduino sketch works in two states: state=0 means stand and wait for orders and state=1 
means move toward the target angle. PotAngle is the variable storing the potentiometer values. 


int state = 1; // State of the motor 
int targetAngle = startPos; // Target Angle 
int potAngle; // Current angle of the potentiometer 


In the setup() function, you only need to start the serial communication and define your servo pin 
as output. 


void setup(){ 
Serial. begin(9600) ; 
pinMode (servoPin, OUTPUT); 


The main loop() function is completely engulfed within an “if()” statement that executes once 
every 20 milliseconds. This makes sure that you only talk to the servo within the acceptable range and 
that you don’t send too much serial data and saturate the channel. 


void loop() { 
unsigned long currentMillis = millis(); 
if(currentMillis - previousMillis > interval) { 
previousMillis = currentMillis; 


The first thing you do in the loop is read the value of the potentiometer and print it out to the serial 
port. After this, you perform a data validation check. If the read value is out of your range (100-824), you 
move the potentiometer in toward your range and you set the state to zero so it stops moving. 


potAngle = analogRead(0); // Read the potentiometer 
Serial.print1n(potAngle); // Send the pot value to Processing 
if(potAngle < startPos){ // If you have 

state = 0; 

updateServo(servoPin, forward) ; 

Serial.println("start"); 


if(potAngle > endPos){ 
state = 0; 
updateServo(servoPin, backward) ; 
Serial.printIn("end"); 


Now you need to check if you have any data in your serial buffer. You will implement a separate 
function for this, so here you simply have to call it. Finally, if your state is 1, you call the function 
goTo(targetAngle), which moves the servo towards your goal. 


checkSerial(); // Check for values in the serial buffer 
if(state==1) { 
goTo(targetAngle) ; 


The function goTo() checks whether the current reading from the potentiometer is higher or lower 
than the target angle and updates the servo accordingly. The step of the servo is larger than one unit, so 
you have allowed a buffer zone of 10 units to avoid the oscillation around a target value. If you are within 
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the accepted range from the target, you set your state to 0 and wait for orders and then you senda 
message through the serial channel saying that you have reached your intended position. 


void goTo(int angle) { 
if(potAngle-angle < -10) { 
updateServo(servoPin, forward) ; 


else if(potAngle - angle > 10){ 
updateServo(servoPin, backward) ; 


else { 
Serial.println("arrived"); 
state=0; 
} 
} 


The orders you are waiting for will come via serial communication. In the checkSerial() function, 
you look into the serial buffer, and if there is at least two data, you read the first one. If this happens to be 
the trigger character ‘S’, you check the next one, which you interpret as the new target angle for the 
turntable. You map this value from 0-255 to the whole range of the potentiometer, and you set your state 
to 1, which allows the turntable to move toward this new value. 


void checkSerial(){ 
if (Serial.available() > 1) { // If data is available to read, 
char trigger = Serial.read(); 
if(trigger == 'S'){ 
state = 1; 
int newAngle = Serial.read(); 
targetAngle = (int)map(newAngle,0,255,startPos,endPos) ; 


The updateServo() function is the same as that used throughout the book. You write a high signal to 
the servo pin for the amount of microseconds that you have passed as an argument. 


void updateServo (int pin, int pulse){ 
digitalWrite(pin, HIGH); 
delayMicroseconds(pulse) ; 
digitalWrite(pin, LOW); 


This simple code allows you to control the rotation of the turntable from any serial-enabled 
software (In your case, Processing). On top of that, this Arduino code is sending out messages with the 
current angle of the turntable and whether the target position has been reached. Let’s see how to use this 
data and your turntable to scan 3D objects. 


Processing Code 


You are going to write a slightly complex piece of code that will communicate with the Arduino board 
and interpret the incoming data to reconstruct a three-dimensional model of the objects standing on the 
turntable. We covered the geometrical principles behind the patching of the different scans at the 
beginning of the chapter, but we will expand on this in the pages that follow. 
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We looked for a complex and colorful shape for this example and, because we are writing this in 
November, the first thing we found was a little figure of Santa in a dollar store (Figure 12-20). We will be 
using this figure for the rest of the chapter. If you can find one too, you’ll have a complete, full-color 
model of your favorite Christmas gift-bringer inside of your computer by the end of the chapter, as per 
Figure 12-21. Let’s start coding! 


: 


Figure 12-20. Our example model 


Figure 12-21. Our example model in scanning space 
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Variable Declaration 


The imports are the usual ones: Serial, OpenGL, Simple-OpenNI, and KinectOrbit. You define a 
Boolean variable called serial that initializes the serial ports only if it is true. This is useful if you want to 
test stuff with the serial cable disconnected and you don’t want to get those nasty errors. If you want to 
play with the sketch without connecting the Arduino, change it to false. The String turnTableAngle 
holds the string coming from Arduino containing the current angle of the turntable. 


import processing.serial.*; 

import processing.opengl.*; 

import SimpleOpenNI.*; 

import kinectOrbit.*; 

// Initialize Orbit and simple-openni Objects 

KinectOrbit myOrbit; 

SimpleOpenNI kinect; 

// Serial Parameters 

Serial myPort; // Initialize the Serial Object 

boolean serial = true; // Define if you're using serial communication 
String turnTableAngle = "0"; // Variable for string coming from Arduino 


You initialize four ArrayLists. The first two, ScanPoints and scanColors, contain the current 
scanned points and their colors. This means the points being scanned currently by the Kinect and 
contained within your scanning volume. The ArrayLists objectPoints and objectColors contain all the 
points that configure your scanned objects patched together. These are the points that will be exported 
in .ply format. 


// Initialize the ArrayLists for the pointClouds and the colors associated 
ArrayList<PVector> scanPoints = new ArrayList<PVector>(); // PointCloud 
ArrayList<PVector> scanColors = new ArrayList<PVector>(); // Object Colors 
ArrayList<PVector> objectPoints = new ArrayList<PVector>(); // PointCloud 
ArrayList<PVector> objectColors = new ArrayList<PVector>(); // Object Colors 


The following variables define the “model space,” the volume that contains your objects. The float 
baseHeight is the height of your base from your Kinect coordinate system. In our case, it was placed 
67mm under the Kinect camera. modelWidth and modelHeight are the dimensions of the model space. 
Anything that falls within this volume will be scanned. 


// Scanning Space Variables 

float baseHeight = -67; // Height of the Model's base 
float modelWidth = 400; 

float modelHeight = 400; 

PVector axis = new PVector(0, baseHeight, 1050); 


You need to define some additional variables. The scanLines parameter defines how many rows of 
the Kinect depth image you use at every scan (Figure 12-22). Think of a traditional structured 3D 
scanner. A single line is scanned at every angle. This parameter can be set to low values if you have a 
pretty symmetrical geometry around its center. Low scanLines values need to be used with a high 
number of scans, defined by the size of shotNumber[ ]. Play with these two parameters until you achieve 
a smooth result in your point cloud. In our example, we used a high number of lines, 200, and only three 
shots or patches. 
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Figure 12-22. The scanLines parameter set to 5 (left), 25 (center), and 65 (right) 


The variable scanRes defines the number of pixels you take from your scan. One is the higher 
resolution. If you chose a higher number than 1, you will skip a number of pixels in every scan. The 
Boolean variables and currentShot integer are used to drive the flow of the sketch depending on the 
incoming data. 


// Scan Parameters 

int scanLines = 200; 

int scanRes = 1; 

boolean scanning; 

boolean arrived; 

float[] shotNumber = new float[3]; 
int currentShot = 0; 


Setup and Draw Functions 


Within the setup() function, you initialize all your KinectOrbit, Simple-OpenNI, and serial objects. 
You also need to work out the angle corresponding to every shot you want to take. The idea is to divide 
the 365 degrees of the circle, or 2*PI radians, into the number of shots specified by the size of the 
shotNumber[ ] array. Later, you send these angles to the Arduino and you take a 3D shot from every one of 
them. Lastly, you move the turntable to the start position, or angle zero, so it’s ready to scan. 


public void setup() { 
size(800, 600, OPENGL); 
// Orbit 
myOrbit = new KinectOrbit(this, 0, "kinect"); 
myOrbit.drawCS(true) ; 
myOrbit.drawGizmo(true) ; 
myOrbit.setCSScale(200) ; 
myOrbit.drawGround(true) ; 
// Simple-openni 
kinect = new SimpleOpenNI (this) ; 
kinect.setMirror(false) ; 
kinect.enableDepth(); 
kinect.enableRGB(); 
kinect.alternativeViewPointDepthToImage() ; 


// Serial Communication 

if (serial) { 
String portName = Serial.list()[0]; // Get the first port 
myPort = new Serial(this, portName, 9600); 
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// don't generate a serialEvent() unless you get a newline 
// character: 
myPort.bufferUntil('\n'); 


for (int i = 0 


3 i < shotNumber.length; i++) { 
shotNumber[i] = 


i * (2 * PI) / shotNumber. length; 


if(serial) { moveTable(o); } 


Within the draw() function, you first update the Kinect data and start the Orbit loop. Pack all the 
different commands into functions so the workflow is easier to understand. 


public void draw() { 
kinect.update(); // Update Kinect data 
background(0) ; 


myOrbit.pushOrbit(this); // Start Orbiting 


First, you draw the global point cloud with the drawPointCloud() function, which takes an integer as 
parameter defining the resolution of the point cloud. As you're not very interested in the global point 
cloud, you speed things up by only drawing one every five points. 


drawPointCloud(5); 


Then you update the object points, passing the scanLines and scanRes defined previously as 
parameters. As you will see later, this function is in charge of transforming the points to the global 
coordinate system. 


updateObject(scanLines, scanRes) ; 


You have two Boolean values defining if you are in the scanning process and whether you have 
reached the next scanning position. If both are true, you take a shot with the function scan(). 


if (arrived && scanning) { scan(); } 


The last step is to draw the objects, the bounding box, and the camera frustum. And of course, close 
the Kinect Orbit loop. 


drawObjects(); 

drawBoundingBox(); // Draw Box Around Scanned Objects 
kinect.drawCamFrustum(); // Draw the Kinect cam 
myOrbit.popOrbit(this); // Stop Orbiting 


Additional Functions 


The drawPointCloud() function is similar to the one previously used to visualize the raw point cloud 
from the Kinect. You take the depth map, bring it to real-world coordinates, and display every point on 
screen. Skip the number of points defined by the steps parameter. The dim, background points on 
Figure 12-23 are the result of this function. 


void drawPointCloud(int steps) { 
// draw the 3D point depth map 
int index; 
PVector realWorldPoint; 
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stroke(255); 


for (int y = 0; y < kinect.depthHeight(); y += steps) { 
for (int x = 0; x < kinect.depthWidth(); x += steps) { 
index = x + y * kinect.depthWidth(); 
realWorldPoint = kinect.depthMapRealWorld() [index]; 
stroke(150); 
point(realWorldPoint.x, realWorldPoint.y, realWorldPoint.z); 


Figure 12-23. Scanned figure in the bounding box 


TURNTABLE SCANNER 


The drawO0bjects() function displays on screen the point cloud defining the object being scanned. 


This function has two parts: first, it displays the points stored in the objectPoints ArrayList, which are 
the object points patched together. Then it displays the points stored in the scanPoints ArrayList, which 


are the points being currently scanned. All these points are displayed in their real color and with 


strokeWeight = 4, so they stand out visually, as you can see on Figure 12-23. 


void drawObjects() { 
pushStyle(); 
strokeWeight (4); 


for (int i = 1; i < objectPoints.size(); i++) { 
stroke(objectColors.get(i).x, objectColors.get(i).y, objectColors.get(i).z); 
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point (objectPoints.get(i).x, objectPoints.get(i).y, objectPoints.get(i).z + axis.z); 


for (int i = 1; i < scanPoints.size(); i++) { 
stroke(scanColors.get(i).x, scanColors.get(i).y, scanColors.get(i).z); 
point(scanPoints.get(i).x, scanPoints.get(i).y, scanPoints.get(i).z + axis.z); 


popStyle(); 


The drawBoundingBox() function displays the limits of the scanning volume. Anything contained 
within this volume is scanned. 


void drawBoundingBox() { 
stroke(255, 0, 0); 
line(axis.x, axis.y, axis.z, axis.x, axis.y + 100, axis.z); 
noFill(); 
pushMatrix(); 
translate(axis.x, axis.x + baseHeight + modelHeight / 2, axis.z); 
box(modelWidth, modelHeight, modelWidth) ; 
popMatrix(); 


The scan() function is in charge of storing the current point cloud into the object point cloud. You 
need to avoid multiplying the number of points unnecessarily, so you implement a loop in which you 
check the distance from each new point to every point already stored in the objectPoints ArrayList. If 
the distance from the new point to any of the previously stored points is under a certain threshold (Imm 
in our example), you won’t add this point to the object point cloud. Otherwise, you add the point and the 
corresponding color to the object ArrayLists. 


void scan() { 
for (PVector v : scanPoints) { 
boolean newPoint = true; 
for (PVector w : objectPoints) { 
if (v.dist(w) < 1) 
newPoint = false; 


if (newPoint) { 
objectPoints.add(v.get()); 
int index = scanPoints.indexOf(v); 
objectColors.add(scanColors.get(index).get()); 
} 
} 


After taking the shot and adding it to the object point cloud, check if the shot taken was the last of 
the list. If the shot is not the last one, you tell the turntable to move to the next position and you set the 
“arrived” Boolean to false. This prevents the sketch from taking any other shot before you get an 
“arrived” signal from Arduino indicating you have reached the target position. If the shot happens to be 
the last one, you set the scanning Boolean to false, declaring the end of the scanning process. Figure 
12-24 shows the three steps in this scanning process. 


if (currentShot < shotNumber.length-1) { 
currentShot++; 
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moveTable(shotNumber[currentShot ]); 
println("new angle = " + shotNumber[currentShot]); 
println(currentShot) ; 


print1n(shotNumber) ; 
else { 
scanning = false; 


arrived = false; 


} 


Figure 12-24. Three-step scan 


The updateObject() function is where the computation of the world coordinates of the scanned 
points takes place. You declare an integer to store the vertex index and a PVector to store the real 
coordinates of the current point. Then you clear the ArrayLists of your scanned points so you can update 
them from scratch. 


void updateObject(int scanWidth, int step) { 
int index; 
PVector realWorldPoint; 
scanPoints.clear(); 
scanColors.clear(); 


You need to know the current angle of the turntable in order to compute the global coordinates of 
the points. You work out this angle by mapping the integer value of your turnTableAngle string from its 
range (100-824) to the 365-degree range in radians (0-2*PI). Remember that turnTableAngle is the string 
coming from Arduino, so it changes every time you rotate your turntable. 


float angle = map(Integer.valueOf(turnTableAngle), 100, 824, 2 * PI, 0); 
The next lines draw a line at the base of the bounding box to indicate the rotation of the turntable. 


pushMatrix(); 

translate(axis.x, axis.y, axis.z); 
rotateY (angle) ; 

line(0, 0, 100, 0); 

popMatrix(); 


Now, you run in a nested loop through your depth map pixels, extracting the real-world coordinates 
of every point and its color. 
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int xMin = (int) (kinect.depthWidth() / 2 - scanWidth / 2); int xMax = (int) 
(kinect.depthWidth() / 2 + scanWidth / 2); 
for (int y = 0; y < kinect.depthHeight(); y += step) { 
for (int x = xMin; x < xMax; x += step) 
index = x + (y * kinect.depthWidth()); 
realWorldPoint = kinect.depthMapRealWorld() [index]; 
color pointCol = kinect.rgbImage().pixels[ index]; 


If the current point is contained within the defined scanning volume or bounding box (this is what 
the scary-looking “if()” statements check), you create the PVector rotatedPoint to store the global 
coordinates of the point. 


if (realWorldPoint.y < modelHeight + baseHeight && realWorldPoint.y > baseHeight) { 
if (abs(realWorldPoint.x - axis.x) < modelWidth / 2) { // Check x 
if (realWorldPoint.z < axis.z + modelWidth / 2 && realWorldPoint.z > axis.z - 
modelWidth / 2) { // Check z 


PVector rotatedPoint; 


The coordinate system transformation process happens in two steps. First, you need to get the 
coordinates of the vector from the center of the turntable. The axis vector defined the coordinates of the 
center of the turntable from the Kinect coordinate system, so you only need to subtract the coordinates 
of the axis vector to the real-world point coordinates to get the transformed vector. Then you need to 
rotate the point around the y-axis by the current angle of your turntable. You use the function 
vecRotY()to do this transformation (Figure 12-25). 


realWorldPoint.z -= axis.z; 
realWorldPoint.x -= axis.x; 
rotatedPoint = vecRotY(realWorldPoint, angle); 


Figure 12-25. Coordinate system transformations 
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Now your rotatedPoint should contain the real-world coordinates of the scanned point, so you can 
add it to the scanPoints ArrayList. You also want to add its color to the scanColors ArrayList so you can 
retrieve it later. Finally, you close all the curly brackets: three for the if() statements, two for the for() 
loops, and the last one to close the function updateObject() function. 


scanPoints.add(rotatedPoint.get()); 
scanColors.add(new PVector(red(pointCol), green(pointCol), blue(pointCol))); 


The vecRotY() function returns the PVector resulting from the operation of rotating of the input 
PVector by the input angle. 


PVector vecRotY(PVector vecIn, float phi) { 
// Rotate the vector around the y-axis 
PVector rotatedVec = new PVector(); 
rotatedVec.x = vecIn.x * cos(phi) - vecIn.z * sin(phi); 
rotatedVec.z = vecIn.x * sin(phi) + vecIn.z * cos(phi); 
rotatedVec.y = vecIn.y; 
return rotatedVec; 


You use the function moveTable(float angle) every time you want to send a new position to your 
turntable. The input angle is in degrees (0-360). This function sends a trigger character ‘S’ through the 
serial channel to indicate that the communication has started. Then you send the input angle remapped 
to one byte of information (0-255). It prints out the new angle for information. 


void moveTable(float angle) { 
myPort .write('S'); 
myPort.write((int) map(angle, 0, 2*PI, 0, 255)); 


println("new angle = "+ angle); 


All the preceding functions are called from the draw() loop or from other functions. You also need to 
include two additional functions that you won't be calling explicitly but will be called by Processing in 
due time. The first one is the serialEvent() function. It is called every time you get a new line character 
in your serial buffer (you specified this in the setup()function). If you receive a string, you trim off any 
white spaces and you check the string. If you receive a “start” or “end” message, you just display it on 
the console. If you receive an “arrived” message, you also set your Boolean arrived to true so you know 
that you have reached the target position. 

If the message is not one of the preceding ones, that means you should be receiving the current 
angle of the turntable, so you update your turnTableAngle string to the incoming string. 


public void serialEvent(Serial myPort) { 

// get the ASCII string: 
String inString = myPort.readStringUntil('\n'); 
if (inString != null) { 

// trim off any whitespace: 

inString = trim(inString) ; 

if (inString.equals("end")) { 

println("end"); 
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else if (inString.equals("start")) { 
println("start"); 


else if (inString.equals("arrived")) { 
arrived = true; 
println("arrived"); 


else { 
turnTableAngle = inString; 
} 
} 
} 


The keyPressed() function is called whenever you press a key on your keyboard. You assign several 
actions to specific keys, as described in the following code. 


public void keyPressed() { 
switch (key) { 
case 'r': // Send the turntable to start position 


moveTable(0); 
scanning = false; 
break; 


case 's': // Start scanning 
objectPoints.clear(); 
objectColors.clear(); 
currentShot = 0; 
scanning = true; 
arrived = false; 
moveTable(0); 
break; 

case 'c': // Clear the object points 
objectPoints.clear(); 
objectColors.clear(); 


break; 

case ‘e': // Export the object points 
exportPly('0'); 
break; 


case 'm': // Move the turntable to the x mouse position 
moveTable(map(mouseX, 0, width, 0, 360)); 
scanning = false; 


break; 

case '+': // Increment the number of scanned lines 
scanLines++; 
println(scanLines) ; 
break; 

case '-': // Decrease the number of scanned lines 
scanLines--; 
println(scanLines) ; 
break; 

} 

‘i 


Note that you implement mouse actions for moving to the start position; also, if you press the 
[S]key, you start the scan from the start position. The turntable is first sent to the zero position and then 
you start scanning. 
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The M key helps you control the turntable. Pressing it sends the turntable to the angle defined by 
the x-coordinate of your mouse on the Processing frame (remapped from 0-width to 0-360). For 
example, pressing M while your mouse is on the left edge of the frame sends the turntable to 0 degrees, 
and pressing it while the mouse is on the right edge sends it to 360 degrees. 

The key E calls the function exportP1y to export the point cloud. This leads to the next step. At the 
moment, you have being able to scan a figure all around and have on your screen a full point cloud 
defining its shape (Figure 12-26) . But remember you wanted to convert this point cloud to a mesh so 
you can bring it to 3D modeling software or 3D print it. You are going to do this with another open 
source software package, so you need to be able to export a file recognizable by this software. You will 
use the polygon file format, or .ply, for this exporting routine, which you will implement yourself. You'll 
see how in the next section. 


Figure 12-26. Figure after 360-degree scan 


Exporting the Point Cloud 


As stated in the introduction, you are going to mesh the point cloud using an open source software 
package called Meshlab, which can import unstructured point clouds in two formats, .ply and obj. 
There are several Processing libraries that allow you to export these formats, but exporting a .pl]y file is 
so simple that you are going to implement your own file exporter so you can control every datum you 
send out. 

The .ply extension corresponds to polygon file format, or stanford triangle format. This format can 
be stored in ASCII and binary files. You will use ASCII so you can actually write your data directly to an 
external file and then read it from Meshlab. 
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The .ply format starts with a header formed by several lines. It starts by declaring “I am a ply file” 
with the word “ply” written on the first line. Then it needs a line indicating the type of . ply file. You 
indicate that you are using ASCII and the version 1.0. You then insert a comment indicating that you are 
looking at your Processing-exported .p1ly file. Note that comments start with the word “comment”— 
pretty literal, eh? 


Note The next lines are not Processing code, but the lines that will be written on your .ply file! 


ply . 
format ascii 1.0 
comment This is your Processing ply file 


These three first lines are constant in every file you export. You could add a comment that changes 
with the name of the file or any other information. If you want to add a comment, just make sure your 
line starts with the word “comment”. 

The next lines, still within the header, declare the number of vertices, and the properties you are 
exporting for each of them. In your case, you are interested in the coordinates and color, so you declare 
six properties: x, y, z, red, green, and blue. You now close the header. 


element vertex 17840 
property float x 
property float y 
property float z 
property uchar red 
property uchar green 
property uchar blue 
end_header 


After closing the header, you have to write the values of all the properties stated for each vertex, so 
you get a long list of numbers like this: 


0.0032535514 0.1112856 0.017800406 125 1 1 
0.005102699 0.1112856 0.017655682 127 1 81 
-0.0022937502 0.10943084 0.018234566 130 1 1 


Each line represents the six properties declared for each vertex in the order you declared them and 
separated by a white space. Let’s have a look at the code you need to implement to effectively producing 
.ply files containing the object point cloud that you previously generated. 


Note In this case, you are only exporting points, but .ply files also allow you to export edges and faces. If you 
wanted to export edges and faces, you would need to add some lines to your header defining the number of edges 
and faces in your model and then a list of faces and a list of edges. You can find information on the .ply format at 
http: //paulbourke.net/dataformats/ply. 
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The exportPly Function 


First, you declare a PrintWriter object. This class prints formatted representations of objects to a text- 
output stream. Then you declare the name of the output file and you call the method createWriter(), 
passing file name in the data path as a parameter. 


void exportPly(char key) { 
PrintWriter output; 
String viewPointFileName; 
viewPointFileName = "myPoints" + key + ".ply"; 
output = createWriter(dataPath(viewPointFileName) ) ; 


Now you need to print out all the header lines. The number of vertices is extracted from the size of 
the objectPoints ArrayList. 


output.println("ply"); 

output.println("format ascii 1.0"); 

output.println("comment This is your Processing ply file"); 
output.println("element vertex " + (objectPoints.size()-1)); 
output.println("property float x"); 
output.println("property float y"); 
output.println("property float z"); 
output.println("property uchar red"); 
output.println("property uchar green"); 
output.println("property uchar blue"); 

output.println("end_ header"); 


After closing the header, you run through the whole objectPoints ArrayList, and you print out its x, 
y, and z coordinates and the integers representing their color parameters. You want the output to be in 
meters, so you scale down the point coordinates by dividing the values by 1000. 


for (int i = 0; i < objectPoints.size() - 1; i++) { 
output.println((objectPoints.get(i).x / 1000) +" " 

(objectPoints.get(i).y / 1000) +" " 

(objectPoints.get(i).z / 1000) +" " 

(int) objectColors.get(i).x +" " 

(int) objectColors.get(i).y + 

(int) objectColors.get(i).z); 


+ 


t+tet 


When you are done with your point cloud, you flush the PrintWriter object and you close it. Now 
you should have a neat .p1y file in the data path of your Processing file! 


output.flush(); // Write the remaining data 
output.close(); // Finish the file 


You have now gone through the whole process of scanning a three-dimensional object and 
exporting the resulting point cloud to a . ply file. You will next learn how to use this point cloud to 
generate a surface using Meshlab. 
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Surface Reconstruction in Meshlab 


Surface reconstruction from unorganized point sets is a complicated process requiring even more 
complicated algorithms. There is a lot of research out there on how this process can be performed, and 
luckily for you, there are several open source libraries like CGAL (http: //cgal.org) and open source 
software like Meshlab (http://meshlab. sourceforge.net) that you can use to perform these operations 
without being an accomplished mathematician. 

Meshlab is an open source system for the processing and editing of 3D triangular meshes. It was 
born in 2005 with the intention of helping with the processing of the unstructured models arising in 3D 
scanning. That is pretty much the state you are in: you have 3D scanned a figure and need to process the 
point cloud and reconstruct the implicit surface. 

Meshlab is available for Windows, Mac OSX and Linux, so whatever your OS, you should now go to 
the web site and download the software. You'll start using it right now! 

If you have already installed Meshlab, .ply files should be automatically associated to it, so you can 
double-click your file and it will open in Meshlab. If this is not the case, open Meshlab and import your 
file by clicking the File menu, and then Import Mesh. 

If your file was exported correctly and the number of vertices corresponds to the declared number 
of vertices, when you open the file you should see on screen a beautifully colored point cloud closely 
resembling your scanned object, like the one shown in Figure 12-27. 


Figure 12-27. Point cloud .ply file opened in Meshlab 


If your file contains inconsistencies (like a different number of points declared and printed), 
Meshlab can throw an “Unexpected end of file” error at you. If you click OK, you won’t see anything on 
the screen. Don’t panic! The points are still there, but you have to activate the Points view on the top bar, 
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and then you will see the point cloud. It won’t be colored, though. You can then go to the Render menu 
and choose Colors/Per Vertex. After this, you should see an image similar to the previous one. 

Now orbit around your point cloud and identify any unwanted points. Sometimes Kinect detects 
some points in strange places, and it helps the reconstruction process to get rid of those. Sometimes the 
Kinect is not perfectly calibrated with the turntable, so you will have slight inconsistencies in the 
patched edges. To delete the unwanted points, select the Select Vertexes tool from the top menu bar. 
You can select several points together by holding the Control or Command key down. After selecting, 
you can click Delete Selected Vertices on the right of the top menu bar (Figure 12-28). 


Figure 12-28. Delete unwanted points 


You are now ready to start the surface reconstruction process. First, you need to compute the 
normal vector on your points, which is the vector perpendicular to the surface of the object at each 
point, pointing out of the object. This provides the reconstruction algorithm information about the 
topology of your object. Go to the Filters/Point Set menu and choose “Compute normal for point sets.” 
This should bring up a menu. Choose 16 as the number of neighbors and click Apply. The process 
shouldn’t take more than a couple of seconds. It may seem like nothing changed, but if you go to the 
Render menu and tick Show Vertex Normals, you should now see the point cloud with all the normals in 
light blue. 

Go to the Filters/Point Set menu again and click Surface Reconstruction: Poisson. In the menu, 
there are several parameters that we won’t have the time to discuss in this book. These parameters have 
an influence on how precise the reconstruction is, how many neighboring points are taking into 
account, and so on. Chose 16 as Octree Depth and 7 as Solver Divide; depending on your object and the 
quality of the scan, the optimum parameters can greatly change, so try to find yours. 
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Figure 12-30. Vertex colors transferred to the mesh 
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After a couple of painfully long minutes, you should get a pretty neat surface like the one in Figure 
12-29. The last step is transferring the colors of the vertices to your mesh, so you can export the fully 
colored mesh to any other software. Go to Filters/Sampling and choose Vertex Attributes Transfer. Make 
sure that the Source Mesh is your point cloud, and the Target Mesh is your Poisson Mesh. Tick only 
Transfer Color and click Apply. The mesh should show now a pretty similar range of colors to the 
original figure (Figure 12-30), and you are done!! 

If your point cloud was not what you would call “neat,” sometimes the Poisson reconstruction will 
throw something much like a potato shape slightly smaller than your point cloud. Don’t be discouraged; 
Meshlab can fix it! In the previous step, instead of transferring the colors only, transfer the geometry as 
well. You will get then a nicely edged shape like the one in the Figure 12-31. 
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Figure 12-31. Vertex geometry and color transferred to the mesh 


Go to “Filters/Smoothing, Fairing, and Deformation” and click HC Laplacian Smooth. Apply the 
filter as many times as needed to obtain a reasonably smooth surface. With a little luck, the result will be 
reasonably good, considering you started from a pretty noisy point cloud. 

One last tip on this process: sometimes you'll get an error on the importation process, the computed 
normal will be inverted, and the figure will look pretty dark. Again, you can use Meshlab to invert your 
face normals. Go to “Filters/ Normals, Curvature, and Orientation” and click Invert Faces Orientation. 
This should make your mesh appear brighter. Check that the light toggle button is on. Sometimes the 
switch is off and you might get the impression that your normals are inverted; if you have just forgotten 
to turn on the light, do it on the top menu bar! 

You have generated a mesh that can be exported to a vast number of file formats compatible with 
CAD packages, 3D printers and other 3D modeling software. Go to File/Export Mesh As and choose the 
file format you need. We exported it as a .ply file with colors and imported it into Blender (see Figure 
12-32), a great open source modeling software freely available from www.blender.org. 
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Figure 12-32. The figure mesh imported into Blender 


If your goal is to replicate your object with a 3D printer, there are plenty of services online that will 
take your STL model, print it (even in color), and send it to you by mail. But, being a tinkerer, you might 
be willing to go for the DIY option and build yourself a RepRap. This is a pretty astonishing concept of 
self-replicating 3D-printer, which means that RepRap is partially made from plastic components that you 
can print using another RepRap. It is certainly the cheapest way to have your own 3D printer, so have a 
look at http://reprap.org/ for plenty of information on where to buy the parts and how to build it. 


Summary 


In this chapter you learned several techniques for working with raw point clouds. You haven’t made use 
of any of the NITE capabilities, so actually this project could be done using any of the available Kinect 
libraries, as long as you have access to the Kinect point data. 

You used gears, a servo, and a multi-turn potentiometer to build a precise turntable that can be 
controlled from Processing and that provides real-time feedback of its rotational state. You acquired 3D 
data with your Kinect and patched the data together using geometrical transforms to form a 360-degree 
scan of an object. You even built your own file-exporting protocol for your point cloud and learned how to 
process the point cloud and perform a Poisson surface reconstruction using Meshlab. The outcome of this 
process is an accurate model of your 3D object that you can export to any 3D modeling software package. 

The possibilities of 360-degree object scanning for architects, character artists, and people working 
in 3D in general are endless. Having this tool means that for less than $200 (including your Kinect!) you 
have a device that can take any three-dimensional shape and bring its geometry and colors into your 
computer to be used by any application. It’s your task now to imagine what to do with it! 


CHAPTER 13 


Kinect-Controlled Delta Robot 


by Enrique Ramos and Ciriaco Castro 


This last chapter of the book will guide you through the process of building and programming your own 
Kinect-controlled delta robot (Figure 13-1). You will learn everything from the inverse kinematics of this 
specific kind of robot to the programming of the hand tracking routines and the process of translating the 
data from the virtual simulation into electrical pulses for the operation of the physical robot. 

Delta robots, or parallel robots, are not only great for industrial use, where they perform a variety 
of tasks at incredibly high speeds, but they are also beautiful to watch and relatively simple to build. 
They rely on the use of parallelograms to translate the rotational movement of three independent 
motors into a precise 3D position of the robot's effector. You are building a basic gripper in this project, 
but the effector could actually be any manipulator you can think of. Imagine this effector changed into 
a magnetic gripper, a laser welding/cutting tip, or a nozzle for 3D printing. After you understand the 
concepts in this project, you will build a platform extendable to any kind of robotic application. 


Figure 13-1. Delta robot simulation and physical robot 
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KINECT-CONTROLLED DELTA ROBOT 


You will need quite a few components and materials for the development of this project, but they are all 
widely available and reasonably priced (see Table 13-1). You will find most components in your local 

hardware store; the motors are standard servos you can find at SparkFun and similar retailers. The ball 
joints are perhaps the most specific pieces for this project, but there are a number of online shops where 


youcan find them. 


Table 13-1. The Parts List 


Part Description Price 

6 aluminum rods M6 diameter, 450 mm length $10.82 

12 ball joints M6 diameter. We found them at reidsupply $3.86 each 
1 aluminum pipe M8 diameter, to be used as a spacer $2.25 

3 aluminum square bars 240 mm length, 20 mm side $8.24 

3 aluminum square bars 60 mm length, 20 mm side $2.12 

1 steel corrugated rod M6 diameter, to be cut in 60 mm length pieces $5.49 
Perspex board Clear acrylic board 3 x 420 x 720 mm $15.80 
Lbrackets 2x 20 mm side, 6 x 50 mm side. From hardware store $4.05 
Timber board MDF 1200 x 350 x 20 mm $5.85 
Power supply We found it at Amazon. $34.99 

3 servo motors Hitec HS-805BB $39.99 each 
2 servo motors Hitec HS-311 $7.99 each 
Extension lead cables 1 for each servo and 1 for LED lights $1.75 each 
Strip board 94 x53 mm copper $1.05 

2 LEDs Basic LED 5 mm $0.35 each 
1 resistor 2200hm You can buy them in packs. $0.25 each 
23 breakaway headers-straight You should have a bunch of these already somewhere $1.50 


in your lab or bedroom. 
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About This Project 


This project was born at the Adaptive Architecture and Computation MSc program at the Bartlett, UCL. We 
created the first version of the Kinect-driven delta robot together with Miriam Dall’Igna under installation 
artist Ruairi Glynn. After finishing the project, we had the opportunity to develop a new, much larger 
version of the installation called “Motive Colloquies” at the prestigious Centre Pompidou in Paris, together 
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with Ruairi Glynn and the London School of Speech and Drama. You can view a first version of the project 
at http: //vimeo.com/20594424 and the performance at Pompidou Centre at http: //vimeo.com/25792567. 


The Delta Robot 


What exactly is a delta robot? A delta robot is a type of parallel robot invented by Reymond Clavel in the 
1980s. The basicidea behind it is the use of parallelograms in the design of the robot’s legs in order to 
keep the orientation of an end platform, or effector, that has three degrees of translational freedom but 
no rotation. The robot consists of only three motors that control the rotation of the three legs, which 
transform these rotations into a specific pose determined by the connectivity and the nature of the joints 
of the structure. 

There are many industrial applications for this type of robot, mainly due to their low inertia, which 
allows high accelerations. This makes delta robots some of the fastest robots ever built; they are 
especially useful for picking and packaging processes. Run a search on the Internet for “delta robot” and 
you will be surprised by the variety of applications and the amazing speeds these robots can achieve. 


Building a Delta Robot 


Let’s start putting things together so you can get your fantastic delta robot working. As mentioned, your 
delta robot has three limbs (even though there are some four-legged members of the family) attached toa 
fix base on one end and to the robot’s effector on the other end. The fix base accommodates three servos 
and the Arduino board; it is the only static part of the robot. The legs constitute a kinematic chain that 
translates the servos’ rotational states into specific spatial positions of the effector, which is composed of 
a rotating base anda gripper, controlled by two small servos. 


The Legs 


In order to describe the robot’s limbs, we’re going to use an analogy of a human leg. Each leg is composed 
of “thigh” (one strong piece that stands the torque coming from the motor), “shin” (two parallel pieces that 
are connected to the effector and the thigh), that are connected through a “knee” consisting of ball joints. 
These legs connect to the effector on an “ankle” joint and to the base on the “hip,” which is the joint 
controlled by the servo motors. 

For the thigh part, we selected hollow square sections due to their higher resilience to bending 
moment and torsion. Before starting to build the first limb, you need to drill small holes on these bars 
in order to connect them to the servos. These holes should be the same diameter as the bolt that you use 
to connect this bar to the servos. It’s important to be really accurate because this union needs to be as 
strong as possible if you want to achieve a good level of accuracy (Figure 13-2). 


Figure 13-2. Thigh connected to the servo horn 
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You need to measure the position of the holes according to the plastic servo horn. Once you have 
drilled these holes (youneed a minimum of two), you can join the square bar to the servo leg using a 
couple of bolts. On the other end, drill one hole in the perpendicular plane. This time it’s bigger: the 
same diameter as the corrugated rod that you are going to pass through (6 mm diameter). The next job 
is to prepare the shin’s aluminum rods. You have to thread them using a diestock in order to screw 
them into the ball joint (Figure 13-3). If you don’t have the right tool, you can use standard steel 
corrugated rods instead of the aluminum ones. Note that this option may require that you use shorter 
legs or more powerful servos, because of the increase in weight. 


Figure 13-3. Threaded aluminum rod and ball joint ready for assembly 


Once you put together the aluminum rod and the ball joint on each end, repeat the process with 
another aluminum rod, making sure they are exactly the same length. 

Next, you join all these elements together and finish your first leg. The use of ball joints is key for 
this project. The leg’s joints should allow full rotational freedom for the correct movement of the robot. 
Pass the corrugated rod trough the ball joint, the square section bar, and the second ball joint, as shown 
in Figure 13-4. 
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Figure 13-4. Ball joint assembly 
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In order to allow the free rotation of the ball joint, use a series of spacers. First, youscrewin a 
couple of nuts on both sides of the square section bar in order to fix it with the rod, and then you fit a 
short section of the aluminum tube (5 mm will do); this allows for ball joint movement without clashing 
with the corrugated rod. These spacers are fitted before and after the ball joint. Finally, put a nylon nut 
on the ends (Figure 13-5). If youdon’t have nylon nuts around, you can use two nuts, locking them 
together tightly in order fix the articulation. 


Figure 13-5. Legarticulation finished 


You now have one leg ready. Just repeat the same process two times (Figure 13-6), paying 
attention to the position of the holes in the square section bars (dimensions should be consistent on 
the three legs!) and the length of the aluminum rods. These two factors play a key role in the final 
accuracy of the robot’s movements. Once you have your legs ready, move on to building the base. 


Figure 13-6. Legs ready to be assembled to the base and effector 
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The Base 


The base holds the servo motors and is the piece connecting the whole robot to the cantilevered timber 
plank acting as a support. If you are planning to build your robot differently, just pay attention to the 
clearance for the legs. When the robot moves, the legs come higher than the base, so youneed to make 
room for them to move freely. 

You are going to fabricate the base out of Perspex, but any other rigid material will work. We built 
our base using a laser cutter, so we got a pretty neat and accurate shape and holes. If you want to laser- 
cut your base, there are some good online services where you can send the vector file (dwg, ai or pdf) 
and they will ship the finished product. Otherwise, you can use the files from the book’s web site to 
trace over and cut your base witha good old scalpel. 

The base shape is a triangle (Figure 13-7) on which you place each leg’s axis aligned to the middle 
point of each side. Use a triangle of 440 mm per side. The servos are held via L brackets so the rotation 
happens on the right plane. You subtract a rectangular shape in each side in order to allow the thigh to 
lift over the plane of the base. You also chop the triangle vertices to save some Perspex and have a 
smaller base (Figure 13-8). 


Figure 13-7. Base shape 
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Figure 13-8. Laser-cut base and servo with an L bracket 


Once the piece is cut, you just have to screw the servo motors to the brackets, fixing their position; 
pay special attention to the distance that they are fixed from the border, which must be equal (Figure 
13-9). The next step is to screw the brackets that hold the servos to the base. 


Figure 13-9. Assembled base 
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The Effector 


The effector is a triangular piece that moves parallel to the plane of the base (Figure 13-10). You build it 
out of a triangular piece of Perspex with 135mm sides. The aluminum rods are joined to this piece. 

Use the same type of joint that you saw in the union between thigh and shin. Start by fixing the 
small square bars to the triangle. Draw a series of holes in the medians of the triangle anda 
rectangular shape where you insert a new servo motor for the gripper. Don’t forget to also draw the 
holes for fixing the servos to this piece! Once the piece is cut, drill some small holes into the square 
bars, making them match up with the ones in the effector. These square bars stick out of the triangular 
shape in order to have enough room to connect them to the legs. Before fixing them withsome screws 
and nuts, drill a hole in one of the extremes (in the perpendicular plane) to build the same type of joint 
previously described. This hole must be big enough to let an M6 rod pass through it. 


: 


Figure 13-10. Effector drawing and assembly 


Once all the holes are drilled, fix the square bars to the Perspex piece, conforming to the effector. Now 
join it to the legs. Remember to use the same type of node you used previously: square bar in the center, 
nut fixing it on both sides, spacer, ball joint, spacer, nylon nut, or double nut. 

Now that the three legs are joined to the effector, you can join them to the base! Just fix the servo leg 
to the servomotor and screw them in so they don’t come off (believe me, they do). And that’s it! Your 
delta robot is nearly finished. But no robot is complete without an effector end. The robot needs a tool to 
act in the world; you're going to build a gripper, a robotic hand that will catch anything within its reach. 


The Gripper 


The gripper is used by the robot to pick up objects. It’s a really simple mechanism that can open and close 
using just one servo. You will laser-cut the Perspex pieces and assemble them into the gripper 
mechanism. The gripper shown in Figure 13-11 is a modified version of a design from 
http://jjshortcut.wordpress.com, which is a modification on a design from http://www. diniro.net (one 
beautiful example of Internet sharing!). We altered the design to account for the thickness of the material, 
the specific servos, and the effector size; we also added a circular base. 
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Figure 13-11. Gripper parts 
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Note that all the holes have been marked in order to make the assembly easier and also to allow 
the rectangular shape to host a servo. The rotation of the servo is transmitted to the gears; this closes 
the gripper. The gripper is assembled to a base that allows rotation in the perpendicular plane using 
another servomotor. 

You start by assembling the first gear with the servo horn, then assemble the first servo to the big 
piece, and continue with the rest of pieces, using bolts and nylon nuts or double nuts. Once you have 
the complete mechanism, test it by moving it with your hand. It should have a pretty smooth movement 
(Figure 13-12). Secure all bolts but don’t over tight them. Add some rubber adhesives in order to have a 
more stable grip (Perspex doesn’t have enough friction, so objects just slip away!). 


- - 


Figure 13-12. Half gripper 
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Figure 13-13. Motor assembled on the gripper 


Once your gripper is assembled, connect it to the effector. Remember that you left enough space to 
fix one servo to the effector (Figure 13-13). Then adjust a servo horn to the gripper circular base. After 
that, fix it to the servo and screw it. Lastly, assemble the gripper with the circular base. To avoid too 
much vibration, use two mini L brackets, one on each side. The final piece should look like the one in 
Figure 13-14. 


Figure 13-14. Assembled gripper 


As a final touch, we added a couple of red LEDs on both sides of the gripper simply as an effect 
(Figure 13-15). The quaint face-like effect it conveys is quite theatrical when the robot is in action. 
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Figure 13-15. Assembled gripper with servo and LEDs on board 


Hanging the Robot 


You cantilever the robot using an MDF board that you previously cut in the same shape as the robot’s base 
(Figure 13-16), allowing the legs to go over the base plane. It is very important that you allow this 
movement. If you attach the robot directly to a flat surface (say you screw the base to your ceiling), the 
legs won’t be able to go over the base plane and the robot’s movements will be severely limited. 
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Figure 13-16. Cantilevered MDF board 


Mark the holes and use long screws to fix this MDF board to the base. Once everything is 
assembled, use a timber shelve as a platform and use a couple of clamps to fix the robot hanging in 
space (Figure 13-17). It’s important to measure this height because you need to add a working plane if 
you want to have a surface where youcan leave objects for the robot to pick (Figure 13-18). 


Figure 13-17. Hanging the robot from the board 
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Figure 13-18. Robot setup 


You can use a normal table as a working plane. You place the Kinect under the working plane—at 
the right height to detect your movements but also far enough from the robot as younever know what 
these creatures are capable of! 


Building the Circuit 


Now it’s time to deal with the wiring. You have two LEDs and two servos in the gripper plus three big 
servos for the legs, and youneed to connect them to your main circuit. Connect some extension lead 
cables from the gripper to connect the LEDs and the servos, running it from one leg through cable ties. 


Another option is to use some cable tidy (a plastic spiral that collects all cables inside), shown in Figure 
13-19. 
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Figure 13-19. Cable tidy 


Then you connect shorter extension cables for the three servos on the legs. Note that youneed an 
external 5V power supply for this project; the 5V power supply coming from the Arduino is not stable 
enough for the power requirements of all servos working at the same time. 

The circuit contains five servos (three for the legs and two for the gripper), two LEDs with their 
resistors, your Arduino, and an external power supply. Arduinos pins 9, 10, and 11 control three servos 
for the legs. Pins 5 and6 control the gripper’s servos and pin 3 controls the LEDs, as per Figure 13-20. 
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Figure 13-20. Delta robot circuit 


Figure 13-21. Circuit materials 
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To build the circuit, you are going to use a strip board (Figure 13-21). Start by measuring it and 
soldering the breakaway headers that connect to your Arduino board (Figure 13-22). It’s important that 
you have a good look at the back of the strip board and confirm the direction of the copper strips before 
starting to solder! If you want to give it more stability, youcan add more headers than necessary. The 
important ones are groundand pins 9, 10, 11,5, 6,and3. 


Figure 13-22. Strip board with soldered headers 


After soldering, double check that they are placed correctly by plugging the shield to your Arduino 
board, making sure that it comes in easily. You may have to bend some of the headers witha nipper to 
get the shield properly connected. 

The next step is to solder strips of three headers that are plugged to the female lining from the 
servos. Put them in the same column, leaving a distance between them; leave three rows in between. 
On each pin you get power (red wire), ground (black wire), and signal (yellow wire), as you saw in the 
previous chapters when we introduced servo motors. All servos are going to share ground and power, 
which come from an external power supply; leave the third pin connected independently to the 
corresponding Arduino pin (Figure 13-23). 
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Figure 13-23. Headers for the servos 


After soldering five of them (one for each servo), solder a strip of two for later connecting the LED. 
In this case, you share just the ground (common for all the circuit) so it is shifted respective to the 
previous column; in other words, one pin is in the same column as the ground and the other completely 
independent in another strip. 

Now turn over the strip board and scratch some lines into the copper so you disconnect pins that 
shouldn’t be connected (servos’ signal). Youcan use a scalpel to remove the copper that connects the 
line. Then check that the connection is completely broken by using a multimeter. 

The next step is to connect the ground to the Arduino ground pin (ground is common in the circuit). 
After that, connect each signal pin to the corresponding pin on the Arduino. Now all your servos are 
connected to the Arduino. For the LED, connect a resistor between the strip that is connected with pin 3 
on the Arduino board and the power free pin that you left before for the LED (Figure 13-24). 


Figure 13-24. Finished board 
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The last step is to solder two cables in the strip’s power and ground for the servos. These two 
cables are connected to an external power source. You can use a PC power supply for a smooth 5V 
power out. The only trick is to hack it so it works without being connected to a computer motherboard. 

You will use the so-called paperclip trick. Make sure that the power is off and the power supply is 
disconnected (Figure 13-25); find the 20/24-pin connector that is used to connect the power supply to 
the motherboard. In the 20-pin block, find the green wire; there is only one and beside it are two black 
(ground cables). On the other end, connect the green wire with any of the grounds using a small piece 
of wire; it doesn’t matter which ground you choose as both will work. 
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Figure 13-25. The paperclip trick 


And that’s it! For safety, wrap some electrical tape around this connection so it’s not exposed. Now 
you can pick any thin cable (5V) and connect red (power) and black (ground) to your circuit 

Everything is ready now! Plug your homemade shield to Arduino, plug all the servos in the 
correspondent pins (making sure that they are the correct ones, and that ground, power, and signals 
are correct!). Connect your LED cable (again making sure that you correctly connect ground and 
power!), connect the power and ground to your external power supply (Figure 13-26), and you're ready 
to bring in the code! 


Figure 13-26. Finished circuit 
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Delta Robot Simulation Software 


You now have a (theoretically) working delta robot hanging from a support, and you have built a circuit 
attached to an Arduino board to control the robot. It’s time to give it a brain! This role is fulfilled by your 
Processing sketch, which will tell the three servos to go to certain positions in order to move the effector 
to a precise point in space. You will also send the necessary data to determine the rotation and the state 
of the gripper. You will, of course send all this information to Arduino through serial communication; your 
board is in charge of transmitting the right messages to the right devices at the right times. 

You are going to proceed step by step. First, you are going to develop the delta robot classes and test 
them with a simple example. When you are sure the simulation is working properly, you'll add all the 
Kinect and Serial methods, and then you will implement the Arduino program that will translate the 
serial data into pulses that the servos can understand. But first, we need to clarify an important concept. 


Inverse Kinematics 


When you work with robots, you often need to make use of inverse kinematics. But what on Earthis that? 
When you have a jointed, flexible object, or kinematic chain (like your delta robot, or any other articulated 
robot), there are two problems that you usually need to solve when studying its movements. 


e If I move all the motors or actuators in the structure to certain states, what is the 
resulting pose of the robot? 


¢  IfI want to achieve a specific pose, what are the required states of my actuators in 
order to achieve it? 


The first question can be answered by forward kinematics, the second one by inverse kinematics. In 
your case, what you want is to be able to drive the robot’s effector to specific points in space so it can 
follow your hand; this is within the realm of inverse kinematics. You have a pretty straightforward 
structure, driven by only three servos (the other two are unrelated to the robot’s position in space), but 
you need to find out how the three independent spatial coordinates of x, y, and z that define the desired 
position of the effector translate back through the kinematic chain to the three rotations of the servos. 
This is achieved by analyzing the connection and geometry of the robot and the constraints this 
geometry imposes on the robot’s kinematics. 

The goal of the inverse kinematics of a leg is to find the servo rotation necessary to achieve a 
specific effector position in space. Let’s assume for a second you are looking at a simpler, two- 
dimensional problem. If youneed to find out the servo angle for the diagram in Figure 13-27, the 
answer is obtained through planar geometry. The code that gives you the answer is the following: 


float c = dist(posTemp.x + effectorSize, posTemp.y, baseSize, 0); 
float alpha = acos((-a2 + thigh * thigh + c * c) / (2 * thigh * c)); 
float beta = -atan2(posTemp.y, posTemp.x); 

servoAngle = alpha - beta; 
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Figure 13-27. Delta robot’s inverse kinematics in 2D and 3D 


But you are not in a two-dimensional world, so you need to resolve the problem for a three- 


dimensional structure. If you look closely at the Figure 13-27, youcan see that the problem can be reduced 
to a two-dimensional problem in which the dimension of the shin is reduced to its projection on the plane 


of the servo rotation. This projection, called a2 in the code, is obtained by simple triangle theory. 
float a2 = shin * shin - posTemp.z * posTemp.z; 


Once a2 is known, you only need to substitute it for the shin in the previous equations and you get 
the answer to the three-dimensional problem. 


float c = dist(posTemp.x + effectorSize, posTemp.y, baseSize, 0); 
float alpha = acos((-a2 + thigh * thigh + c * c) / (2 * thigh * c)); 
float beta = -atan2(posTemp.y, posTemp.x); 

servoAngle = alpha - beta; 


Note This has been a very brief introduction to delta robot’s inverse kinematics, but there are plenty of 
resources on the Internet where you can find more information on this topic. Another advantage of delta robots is 
the extent to which they have been studied and the availability of good online information about them. 


Now you need to structure your delta robot classes. 
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DeltaRobot Class 


The delta robot simulation consists of the DeltaRobot class (Figure 13-28), containing the main 
parameters and routines, anda DeltaLeg class, where you deal with the inverse kinematics and the 
drawing functions for each leg. Let’s start with the DeltaRobot class. Youneeda series of fields defining 
the position of the effector ateach moment in time, anda zeroPos PVector that establishes the position 
from which you define the movements of your effector. 


DeltaLeg[0] 


DeltaLeg[1] 


DeltaLeg[2] 


DeltaRobot Class DeltaLeg Class 
Figure 13-28. Delta robot classes 


You define a variable called numLegs that defines the number of legs of your robot. Yes, you can 
actually increase the number of legs to 4, 5, or any higher number. The fact that there is no 23-legged 
parallel robot out there is because any high number of legs would just be a waste of material (three 
being the minimum necessary number of legs for stability). Having said that, one of the fastest 
commercial delta robots, the Adept Quattro, is based on a four-leg architecture, so feel free to play with 
the parameter. The results are certainly curious (Figure 13-29). 


class DeltaRobot { 
PVector posVec = new PVector(); // Position of the Effector 
PVector zeroPos; 
int numLegs = 3; // Number of legs 
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Figure 13-29. Two distant (and pretty bizarre) cousins of your delta robot, with 5 and 150 legs 


You then set an array called DeltaLeg to contain the three (or any other number of) legs of the 
robot, and an array of floats for the angles of the joints driven by the servos (the upper joints). You also 
declare the variables that set the dimensions of your specific delta robot, and three variables defining 
the maximum span of your robot in the three axes: robotSpanX, robotSpanZ, and maxH). Youneed to 
respect these distances if you don’t want to send unreachable locations to your robot, as this would 
lead to the legs disappearing in the case of the simulation, or worst, a damaged robot if you are driving 
the physical robot. 


DeltaLeg[] leg = new DeltaLeg[numLegs]; // Create an array of deltaLegs 
float[] servoAngles = new float[numLegs]; // Store the angles of each leg 
float thigh, shin, baseSize, effectorSize; // Delta-Robot dimensions 
float gripRot = 100; 

float gripWidth = 100; 

// Maximum dimensions of the Robot space 

float robotSpanX = 500; 

float robotSpanZ = 500; 

float maxH; 


You have come to the main constructor of the DeltaRobot class. It takes as parameters the lengths of 
the hip and ankle elements, and the size of the base and the effector. Note that when you refer to the 
base and effector sizes, you mean the distance between the center of the base/effector and the axis of 
the joint attached to the element (Figure 13-30). 
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Figure 13-30. Base and effector sizes in the sketch 


DeltaRobot(float thigh, float shin, float baseSize, float effectorSize) { 
// Set the variables 
this.thigh = thigh; 
this.shin = shin; 
this.baseSize = baseSize; 
this.effectorSize = effectorSize; 
this.maxH = -(thigh + shin) * 0.9f; // Avoid the effector going out of range 
zeroPos = new PVector(0, maxH / 2, 0); 


The DeltaLegs need to be initialized to the right dimensions, those that you extract from your 
robot’s parameters. Each leg is initialized to a certain angle, one that you find by dividing the full 
circle (2*Pi radians) by the number of legs. In our sketch, we added PI/6 because we wanted to center 
the legs from the Kinect point of view. 


for (int i = 0; i < numLegs; i++) { 
float legAngle = (i * 2 * PI / numLegs) + PI / 6; 
leg[i] = new DeltaLeg(i, legAngle, thigh, shin, baseSize, effectorSize) ; 


Once all your variables are initialized, you add some methods to your main delta robot class. The 
public methods you call from the main sketch are moveTo() and draw(). 

The moveTo() method takes a PVector as a parameter. You use this method to tell the robot to go to 
a specific position from your main sketch. You consider this PVector to be the relative position of the 
robot’s effector to the point specified as zero position. The resulting position of your robot’s effector is 
the result of adding the incoming PVector to the zero position. 


public void moveTo(PVector newPos) { 
posVec.set(PVector.add(newPos, zeroPos)); 


Within this method, and before passing the new position to each leg, you need to make sure that 
the position is a “legal” position, which means a position physically reachable by the robot. The 
position is legal if itis contained within a parallelepiped of the dimensions defined by the variables 
robotSpanx, robotSpanZ, and maxH. If one of your coordinates is out of this virtual volume, trim the value 
to the maximum value of the movement on that axis. 


float xMax 
float xMin 


robotSpanX * 0.5f; 
-robotSpanX * 0.5f; 
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float zMax = robotSpanZ * 0.5f; 

float zMin = -robotSpanZ * 0.5f; 
float yMax = -200; 

float yMin = 2*maxH+200; 

if (posVec.x > xMax) posVec.x = xMax; 
if (posVec.x < xMin) posVec.x = xMin; 
if (posVec.y > yMax) posVec.y = yMax; 
if (posVec.y < yMin) posVec.y = yMin; 
if (posVec.z > zMax) posVec.z = zMax; 
if (posVec.z < zMin) posVec.z = zMin; 


Finally, move each leg to its new position and inquire what angle is necessary to move your 
servos to in order to achieve it. The servo angle is stored in an array for later. 


for (int i = 0; i < numLegs; i++) { 
leg[i].moveTo(posVec); // Move the legs to the new position 
servoAngles[i] = leg[i].servoAngle; // Get the servo angles 


By this step, you have all the data youneed to drive the robot (the three servo angles), so youcan 
now implementa serial communication protocol and start driving your robot straight away. But let’s 
develop the class a little further to include a whole visualization interface for your robot. You will 
ultimately have a virtual model of the delta robot that behaves exactly like the physical model and that 
you can visualize on screen. We think this is important for the project because it’s a good idea to make 
sure everything is working before you output the values to the servos, but the whole project would work 
perfectly without the visual side. We also consider this simulation to be of great help in understanding 
the behavior of the machine, and it will later prove very useful when you bring the Kinect input along. 

The public method draw()is called from your main routine when you want to print your delta robot 
on screen. This method changes the main matrix to a different position so the delta robot is shown ata 
certain height instead of at the origin of coordinates, because later this origin is the Kinect device. 
Then you call eachleg’s own draw() function and the drawEffector() function that draws the gripper. 


public void draw() { 
stroke(50); 
pushMatrix(); 
translate(0, -maxH, 0); 


for (int i = 0; i < numLlegs; i++) { 
leg[i].draw(); 


drawEffector(); 
popMatrix(); 


The drawEffector() function displays the effector and the gripper you have attached to it. We won’t 
go into much detail on this because it is mainly an exercise of changing transformation matrices and 
drawing elements so you end up having a simplified model of your gripper that can be rotated, open, 
and closed at will (Figure 13-31). 
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Figure 13-31. Effector and gripper simulation 


void drawEffector() { 
// Draw the Effector Structure 
stroke(150); 
£i11(150, 50); 
beginShape(); 
for (int i = 0; i < numLegs; i++) { 
vertex(leg[i].ankleVec.x, leg[i].ankleVec.y, 
leg[i].ankleVec.z); 


endShape (CLOSE) ; 
// Draw Gripper 
stroke(200, 200, 255); 
fi11(200, 50); 


// Translate your Coordinate System to the effector position 


pushMatrix(); 
translate(posVec.x, posVec.y - 5, posVec.z); 
rotateX(-PI/2); // Rotate The CS, so you can drwa 


ellipse(0, 0, effectorSize / 1.2f, effectorSize / 1.2f); 
rotate(map(gripRot, 35, 180, -PI / 2, PI / 2)); 


for (int j = -1; j < 2; j += 2) { 
translate(0, 2 * j, 0); 
beginShape(); 
vertex(-30, 0, 0); 
vertex(30, 0, 0); 
vertex(30, 0, -35); 
vertex(15, 0, -50); 
vertex(-15, 0, -50); 
vertex(-30, 0, -35); 
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endShape (CLOSE) ; 


for (int i = -1; i < 2; i += 2) { 
pushMatrix(); 
translate(i * 20, 0, -30); 
rotateXx(PI / 2); 
ellipse(0, 0, 10, 10); 
rotate(i * map(gripWidth, 50, 150, 0, PI / 2.2f)); 
rect(-5, -60, 10, 60); 
translate(0, -50, 0); 
rotate(-i * map(gripWidth, 50, 150, 0, PI / 2.2f)); 
rect(-5, -60, 10, 60); 
popMatrix(); 
} 


} 
popMatrix(); 


There is one more function to add, but you only use in the next example. The updateGrip() method 
can be called from the main sketch whenever you want to change the rotation and width of the gripper. 


public void updateGrip(float gripRot, float gripWidth) { 
this.gripRot = gripRot; 
this.gripWidth = gripWidth; 
} 
} 


deltaLeg Class 


Your main DeltaRobot class is complete now, and you have built the necessary interfaces to drive the 
robot from your main program, but the most important part of the code is yet to be implemented: working 
out the inverse kinematics of the robot and getting the servo angles necessary to achieve the desired 
pose. 

In the deltaRobot class, when moving the robot to a new position, you use a “black box” that gives 
you the servo angles. This black boxis the deltaLeg class. This class has a series of variables defining 
the id of the leg, position of the effector, angle of the servo, angle of the leg, and the real-world 
coordinates of the leg’s joints, which is used for visualization. 


class DeltaLeg { 
int id; // id of the leg 
PVector posVec = new PVector(); // Effector Position 
float servoAngle; // Anlgle between the servo and the XZ plane 
float legAngle; // Y rotation angle of the leg 


// Universal position of the joints 
PVector hipVec, kneeVec, ankleVec; 


float thigh, shin, baseSize, effectorSize; // Sizes of the robot's elements 
DeltaLeg(int id, float legAngle, float thigh, float shin, float base, float effector) { 
this.id = id; 
this.legAngle = legAngle; 
this.baseSize = base; 
this.effectorSize = effector; 


} 
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this.thigh = thigh; 
this.shin = shin; 


The following function, moveTo, functionis actually where the magic happens. This short function 


converts the input position vector into a servo rotation. This is the equivalent of saying that this is the 


fun 
kin 


Vv 


ction dealing with the robot’s inverse kinematics. (We ran through the delta robot’s inverse 
ematics previously, so we just repeat the code here.) 


oid moveTo(PVector thisPos) { 
posVec.set(thisPos) ; 
PVector posTemp = vecRotY(thisPos, -legAngle); 


// find projection of a on the z=0 plane, squared 
float a2 = shin * shin - posTemp.z * posTemp.z; 


// calculate c with respect to base offset 

float c = dist(posTemp.x + effectorSize, posTemp.y, baseSize, 0); 

float alpha = (float) Math.acos((-a2 + thigh * thigh + c * c) / (2 * thigh * c)); 
float beta = -(float) Math.atan2(posTemp.y, posTemp.x); 

servoAngle = alpha - beta; 

getWorldCoordinates(); 


The function getWorldCoordinates() function updates the PVectors defining the joints of the leg 


in world coordinates, so it can draw them on screen. Use the helper functions vecRotY() and vecRotZ() 
to perform the vector rotations. 


} 


} 


void getWorldCoordinates () { 
// Unrotated Vectors of articulations 
hipVec = vecRotY(new PVector(baseSize, 0, 0), legAngle); 
kneeVec = vecRotZ(new PVector(thigh, 0, 0), servoAngle); 
kneeVec = vecRotY(kneeVec, legAngle); 
ankleVec = new PVector(posVec.x + (effectorSize * (float) Math.cos(legAngle)), posVec.y, 
posVec.z - 5 + (effectorSize * (float) Math.sin(legAngle))); 


PVector vecRotY(PVector vecIn, float phi) { 

// Rotates a vector around the universal y-axis 

PVector rotatedVec = new PVector(); 

rotatedVec.x = vecIn.x * cos(phi) - vecIn.z * sin(phi); 
rotatedVec.z = vecIn.x * sin(phi) + vecIn.z * cos(phi); 
rotatedVec.y = vecIn.y; 

return rotatedVec; 


PVector vecRotZ(PVector vecIn, float phi) { 

// Rotates a vector around the universal z-axis 

PVector rotatedVec = new PVector(); 

rotatedVec.x = vecIn.x * cos(phi) - vecIn.y * sin(phi); 
rotatedVec.y = vecIn.x * sin(phi) + vecIn.y * cos(phi); 
rotatedVec.z = vecIn.z; 

return rotatedVec; 
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The public draw() function functionis called from the deltaRobot’s draw() function and takes care 


of displaying the geometry of the leg on screen. Once again, we won’t go into much detail here, but you 
can follow the comments in the code. 


public void draw() { 


// Draw three lines to indicate the plane of each leg 
pushMatrix(); 

translate(0, 0, 0); 

rotateY(-legAngle) ; 

translate(baseSize, 0, 0); 

if (id == 0) stroke(255, 0, 0); 

if (id == 1) stroke(0, 255, 0); 

if (id == 2) stroke(0, 0, 255); 

line(-baseSize / 2, 0, 0, 3 / 2 * baseSize, 0, 0); 
popMatrix(); 


// Draw the Ankle Element 

stroke(150); 

strokeWeight (2); 

line(kneeVec.x, kneeVec.y, kneeVec.z, ankleVec.x, ankleVec.y, 
ankleVec.z); 

stroke(150, 140, 140); 

fi11(50); 

beginShape(); 

vertex(hipVec.x, hipVec.y + 5, hipVec.z); 
vertex(hipVec.x, hipVec.y - 5, hipVec.z); 
vertex(kneeVec.x, kneeVec.y - 5, kneeVec.z); 
vertex(kneeVec.x, kneeVec.y + 5, kneeVec.z); 
endShape(PConstants.CLOSE) ; 

strokeWeight (1); 


// Draw the Hip Element 
stroke(0); 
£i11(255); 


// Align the z axis to the direction of the bar 

PVector dirVec = PVector.sub(kneeVec, hipVec); 

PVector centVec = PVector.add(hipVec, PVector.mult(dirVec, 0.5f)); 
PVector new dir = dirVec.get(); 

PVector new_up = new PVector(0.0f, 0.0f, 1.0f); 
new_up.normalize(); 

PVector crss = dirVec.cross(new_up); 

float theAngle = PVector.angleBetween(new dir, new_up); 
crss.normalize(); 


pushMatrix(); 

translate(centVec.x, centVec.y, centVec.z); 
rotate(-theAngle, crss.x, crss.y, CYss.z); 

// rotate(servoAngle) ; 

box(dirVec.mag() / 50, dirVec.mag() / 50, dirVec.mag()); 
popMatrix(); 
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Driving the Delta Robot Simulation with the Mouse 


You have wrapped all functions you need for driving and visualizing a delta robot in two classes that you 
can add to any Processing sketch, so let’s run a first test. 

You are going to implement the simplest of applications and drive a virtual model of a delta robot 
with your mouse position. Import OpenGL and your Kinect Orbit library; then initialize a deltaRobot 
object to play with. 


import processing.opengl.*; 
import kinectOrbit.KinectOrbit; 


import SimpleOpenNI.*; 


// Initialize Orbit and simple-openni Objects 
KinectOrbit myOrbit; 
SimpleOpenNI kinect; 


// Delta Robot 
DeltaRobot dRobot; 
PVector motionVec; 


public void setup() { 
size(1200, 900, OPENGL); 
smooth(); 


// Orbit 
myOrbit = new KinectOrbit(this, 0, "kinect"); 
myOrbit.setCSScale(100) ; 


// Initialize the Delta Robot to the real dimensions 
dRobot = new DeltaRobot(250, 430, 90, 80); 


Now the only thing youneed to do is, within the orbit loop (so you can rotate around), create a 
motion PVector with your mouse coordinates, move the delta robot to that point in space, and draw it 
on screen to see the result. Done! 


public void draw() { 
background(0) ; 
myOrbit.pushOrbit(this); // Start Orbiting 
motionVec = new PVector(width/2-mouseX, 0, height/2-mouseY);// Set the motion vector 
dRobot.moveTo(motionVec); // Move the robot to the relative motion vector 
dRobot.draw(); // Draw the delta robot in the current view. 
myOrbit.popOrbit(this); // Stop Orbiting 


Run the sketch. You should get a neat image of a delta robot on screen (Figure 13-32). If youmove 
your mouse over the sketch, the robot should move around accordingly. 
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Figure 13-32. Mouse-driven delta robot simulation 


Spend some time with your new virtual toy and be delighted by its movements. When we first saw 
a delta robot in action, we were amazed by the richness of the movements that emerged from sucha 
simple structure. When you have made friends with this quaint creature, you can move on to the next 
step: driving it with your bare hands. 


Kinect-Controlled Delta Robot 


You are going to reuse the two classes you already created to develop a more complex application. The 
goal is to introduce Kinect and NITE’s hand tracking capabilities to drive the delta robot simulation, and 
then implement your own routine to drive the gripper by tilting, opening, and closing your hand (Figure 
13-33). Then you will add a serial communication routine to send the servo state to Arduino so you can 
plug the software straight into your physical robot and drive it with your hand. 
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Figure 13-33. Delta robot driven by hand gestures 


Import the full set of libraries you have been working with (this means Simple-OpenNI, Serial, 
OpenGL, and KinectOrbit) and then declare the corresponding variables. You are using hand tracking, 
so youneed NITE’s Session Manager and Point Control. The Boolean variable serial can be set to 
false if you want to run the sketch without an Arduino board plugged to the computer. 


import processing.opengl.*; 
import processing.serial.*; 
import SimpleOpenNI.*; 

import kinectOrbit.KinectOrbit; 


// Initialize Orbit and simple-openni Objects 
KinectOrbit myOrbit; 

SimpleOpenNI kinect; 

Serial myPort; 

boolean serial = true; 


// NITE 


XnVSessionManager sessionManager; 
XnVPointControl pointControl; 
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// Font for text on screen 
PFont font; 


// Nariables for Hand Detection 

boolean handsTrackFlag; 

PVector handOrigin = new PVector(); 

PVector handVec = new PVector(); 

ArrayList<PVector> handVecList = new ArrayList<PVector>(); 
int handVecListSize = 30; 

PVector[] realWorldPoint; 


You want to be able to control the position of the robot and the state of the gripper, so youneed to 
declare the PVector motionVec and the two floats gripRot and gripWidth. 


// Delta Robot 
DeltaRobot dRobot; 
PVector motionVec; 
float gripRot; 
float gripWidth; 


private float[] serialMsg = new float[5]; // Serial Values sent to Arduino 


The setup() function initializes all the objects you have previously declared and enables all NITE 
capabilities you are going to be using. Note that you are only adding “wave” gesture recognition to the 
Session Manager because you want to have a better control of hand creation in runtime. If you include 
RaiseHand, as soon as your hand enters Kinect’s field of view you trigger a hand creation event; by 
adding only waving, youcan control when and where you start tracking your hand, which is useful in a 
later stage. 

Of course, you also initialize the delta robot object to the dimensions (in mm) of the robot you have 
built and set the serial communication to the first port in your computer where your Arduino is 
connected. 


public void setup() { 
size(800, 600, OPENGL); 
smooth(); 


// Orbit 

myOrbit = new KinectOrbit(this, 0, "kinect"); 
myOrbit.drawCS(true) ; 
myOrbit.drawGizmo(true) ; 
myOrbit.setCSScale(100) ; 


// Simple-openni object 

kinect = new SimpleOpenNI(this) ; 
kinect.setMirror(false) ; 

// enable depthMap generation, hands + gestures 
kinect.enableDepth(); 

kinect.enableGesture(); 

kinect.enableHands(); 


// setup NITE 

sessionManager = kinect.createSessionManager("Wave", "Wave"); 
// Setup NITE.s Hand Point Control 

pointControl = new XnVPointControl(); 
pointControl.RegisterPointCreate(this) ; 
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pointControl.RegisterPointDestroy(this) ; 
pointControl.RegisterPointUpdate(this) ; 


sessionManager .AddListener(pointControl) ; 
// Array to store the scanned points 
realWorldPoint = new PVector[kinect.depthHeight() * kinect.depthWidth()]; 
for (int i = 0; i < realWorldPoint.length; i++) { 
realWorldPoint[i] = new PVector(); 


// Initialize Font 
font = loadFont("SansSerif-12.vlw"); 


// Initialize the Delta Robot to the real dimensions 
dRobot = new DeltaRobot(250, 430, 90, 80); 


if (serial) { 
// Initialize Serial Communication 
String portName = Serial.list()[0]; // This gets the first port on your computer. 
myPort = new Serial(this, portName, 9600); 


} 


Now add the XvN Point Control callback functions, which are pretty similar to the ones you used in 
previous examples for hand tracking. Set your hand tracking flag on or off and update your hand 
PVector and your hand position history ArrayList. 

Add one more line to the onPointCreate function. Because you want to track your hand movements 
after you have waved to the Kinect, you need to set the handOrigin PVector to the point in space where 
the waving took place (Figure 13-34). This allows you to set your motion vector as the relative 
displacement vector of your hand from the hand-creation point. 


Figure 13-34. Hand origin and current hand position 
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public void onPointCreate(XnVHandPointContext pContext) { 
println("onPointCreate:"); 
handsTrackFlag = true; 
handVec.set(pContext.getPtPosition().getX(), pContext.getPtPosition() 
-getY(), pContext.getPtPosition().getZ()); 
handVecList.clear(); 
handVecList.add(handVec.get()); 
handOrigin = handVec.get(); 


} 


public void onPointDestroy(int nID) { 
println("PointDestroy: " + nID); 
handsTrackFlag = false; 


public void onPointUpdate(XnVHandPointContext pContext) { 
handVec.set(pContext.getPtPosition().getX(), pContext.getPtPosition() 
-getY(), pContext.getPtPosition().getZ()); 
handVecList.add(0, handVec.get()); 
if (handVecList.size() >= handVecListSize) { // remove the last point 
handVecList.remove(handVecList.size() - 1); 
} 
} 


The main draw() loop updates the Kinect object and sets a Kinect Orbit loop. Within this loop, you 
update the hand position and draw it, using two specific functions that you implement next. 


public void draw() { 
background(0) ; 
// Update Kinect data 
kinect.update(); 
// update NITE 
kinect .update(sessionManager) ; 
myOrbit.pushOrbit(this); // Start Orbiting 


if (handsTrackFlag) { 
updateHand(); 
drawHand(); 


Just to have an idea of the relative motion between the current position of the hand and the origin 
point (where the hand was created), draw a green line between the two. 


// Draw the origin point, and the line to the current position 
pushStyle(); 

stroke(0, 0, 255); 

strokeWeight (5); 

point(handOrigin.x, handOrigin.y, handOrigin.z); 

popStyle(); 

stroke(0, 255, 0); 

line(handOrigin.x, handOrigin.y, handOrigin.z, handVec.x, handVec.y, 
handVec.z); 


And now youstore that relative motion vector into the motionVec PVector and use it as a parameter 
to move the delta robot to its new position. You then proceed to draw the delta robot and send the serial 
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data. Adda displayText function that prints on screen the data you are sending to the serial port for 
double- checking. 


motionVec = PVector.sub(handVec, handOrigin);// Set the relative motion vector 
dRobot.moveTo(motionVec); // Move the robot to the relative motion vector 
dRobot.draw(); // Draw the delta robot in the current view. 
kinect.drawCamFrustum(); // Draw the Kinect cam 
myOrbit.popOrbit(this); // Stop Orbiting 
if (serial) { 

sendSerialData(); 


displayText(); // Print the data on screen 


Gripper Control 


You want to implement a routine to control the delta robot’s gripper with your hand movements. Here is 
the theory. 

Using NITE, you have already worked out the position of your hand, that is the very center of your 
palm, and youare using it to set the robot’s position in space. Wouldn’t it be cool to open and close the 
gripper by opening and closing your hand? Well, there is a straightforward way of implementing this 
feature. You know your hand is defined by the set of points around the handVec position, so you can 
parse your point cloud and select all the points within a certain distance of your hand vector. After 
some testing, youset 100mm as a reasonable distance. If you display the points that fall within this 
rule, you get a “white glove” (Figure 13-35), which you use to extract the state of your hand. 


Figure 13-35. Hand point cloud 


Once you know what points constitute your hand, you can think about what changes when you 
open and close your hand. No two ways around it: when you open your hand, the total width of your 
point cloud increases, which is to say that the horizontal distance between the rightmost and leftmost 
points (the tips of the thumb and little finger) increases (Figure 13-36). 


377 


CHAPTER 13 !) KINECT-CONTROLLED DELTA ROBOT 


Figure 13-36. Hand width tracking 


In the same way, you can observe that when you tilt your hand forwards and backwards, the total 
height of the point cloud changes, which you can later use to define the rotation of your gripper (Figure 
13-37). 


Figure 13-37. Hand tilt tracking 


You need to find these points and calculate their distances. Here’s the routine. 
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void updateHand() { 
// draw the 3D point depth map 
int steps = 3; // You can speed up the calculation by using less points 
int index; 
stroke(255); 
// Initialize all the PVectors to the barycenter of the hand 
PVector handLeft = handVec.get(); 
PVector handRight = handVec.get(); 
PVector handTop = handVec.get(); 
PVector handBottom = handVec.get(); 


for (int y = 0; y < kinect.depthHeight(); y += steps) { 
for (int x = 0; x < kinect.depthWidth(); x += steps) { 
index = x + y * kinect.depthWidth(); 
realWorldPoint[index] = kinect.depthMapRealWorld()[index].get(); 
if (realWorldPoint[index].dist(handVec) < 100) { 
// Draw poin cloud defining the hand 
point (realWorldPoint[index].x, realWorldPoint[index].y, realWorldPoint[index].z); 
if (realWorldPoint[index].x > handRight.x) handRight = realWorldPoint[index].get(); 
if (realWorldPoint[index].x < handLeft.x) handLeft = realWorldPoint[index].get(); 
if (realWorldPoint[index].y > handTop.y) handTop = realWorldPoint[index].get(); 
if (realWorldPoint[index].y < handBottom.y) handBottom = realWorldPoint[ index].get(); 


After running this loop, you have four PVectors storing the four points you need for your purposes. 
You are going to draw a control gizmo using these points. This gizmo is a cube, the size of which will 
change according to your hand’s width. Likewise, the tilt will try to match the tilt of your hand, based on 
its height. 


// Draw Control Cube 

fi11(100, 100, 200); 

pushMatrix(); 

translate(handVec.x, handVec.y, handVec.z); 
rotateX(radians(handTop.y - handBottom.y)); 

box((handRight.x - handLeft.x) / 2, (handRight.x - handLeft.x) / 2, 
10); 

popMatrix(); 


After running this code ad nauseam, we came up with some numbers that make a good range for 
the hand width and height values. A range of 65-200 works pretty nicely for both parameters. If you 
map this range to 0-255, you will have a pretty smooth value to be passed on to the Arduino board. You 
store the mapped values as gripWidth and gripRot. 


// Set the robot parameters 
gripWidth = lerp(gripWidth, map(handRight.x - handLeft.x, 65, 200, 0, 255), 0.2); 
gripRot = lerp(gripRot, map(handTop.y - handBottom.y, 65, 200, 0, 255), 0.2f); 
dRobot .updateGrip(gripRot, gripWidth) ; 

} 


Include the drawHand() function from previous sketches to keep track of previous hand positions. 


void drawHand() { 
stroke(255, 0, 0); 
pushStyle(); 
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strokeWeight (6) ; 

point(handVec.x, handVec.y, handVec.z); 
popStyle(); 

noFill(); 

Iterator itr = handVecList.iterator(); 
beginShape(); 


while (itr.hasNext ()) { 
PVector p = (PVector) itr.next(); 
vertex(p.x, p.yY, p-Z)3 


endShape(); 


Sending the Data to Arduino 


By now you have all the data you need to control your robot, so you can proceed to sending these 
values to the Arduino board. You will develop the Arduino code in the next section. Remember that you 
were calling a sendSerialData() function from your main draw() loop, so it’s time to implement this 
function. 

First, send the trigger character ‘xX’ to indicate the beginning of the serial message. Then you have 
five values to send: three rotations for each arm’s servo position, the gripper’s rotation, and gripper’s 
width. 

Your servos accept a range from 500 to 2500, which gives you 2000 different possible positions. But 
Arduino reads the incoming serial data one byte at a time and, as one byte can only hold 256 values, 
that is the maximum resolution for your servo rotation that you can transmit in one single message. 
This is much lower than the resolution that your servos can use. 

You want to match the resolution of your messages to the 2000 possible servo positions to get as 
smooth a movement of the robot as you can. The way you solve this problem is by splitting your integer 
into two single-byte numbers, sending the two bytes, and then recomposing the integer on the other 
end of the line (i.e. in the Arduino). What you are doing here is developing a protocol, as youlearned in 
Chapter 4. To perform this splitting, you are going to make use of bitwise operations. 


Note Bitwise operations handle binary numerals at the level of their individual bits. They are fast, primitive 
actions, directly supported by the processor. A detailed explanation of bitwise operations is beyond the scope of 
this book, but you can find more information on www. cplusplus .com/doc/tutorial/operators. 


First, you write the trigger character ‘xX’ to the serial buffer to indicate the start of the 
communication. For each leg, you work out your angle as an integer, mapped to the range 0-2000 in 
order to match its range to the range of the servos. You then store itin the serialMessage[] array to be 
displayed on screen later. 

Now you know the integer serialAngle that you want to send via serial. You can proceed to 
decompose it into its more significant byte (MSB) and its less significant byte (LSB), which you send as 
two consecutive serial messages. 

The bitwise operation & OxFF masks all but the lowest eight bits, so youcan use this to extract the 
LSB. The operation >> 8 shifts all bits eight places to the right, discarding the lowest eight bits. You can 
combine this operation with & OxFF to get the MSB. 
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You cast the two values to byte, because both operations return integers. After this, you are ready 
to write the two values to your serial port. 


void sendSerialData() { 
myPort .write('X'); 
for (int i=0;i<dRobot.numLegs;it++) { 
int serialAngle = (int)map(dRobot.servoAngles[i], radians(-90), radians(90), 0, 2000); 
serialMsg[i] = serialAngle; 
byte MSB = (byte)((serialAngle >> 8) & OxFF); 
byte LSB = (byte)(serialAngle & OxFF); 


myPort .write(MSB) ; 
myPort .write(LSB) ; 


For the grip rotation and width, 256 values are enough. If you are a perfectionist, youcan take up 
the challenge of sending all the values as pairs of bytes. 


myPort .write((int)(gripRot)); 
serialMsg[3] = (int) (gripRot); 
myPort .write((int)(gripWidth)); 
serialMsg[4] = (int) (gripWidth) ; 


Finally, you display on screen the values that you are passing to the Arduino. This is good practice 
for debugging purposes. 


void displayText() { 


£i11(255); 

textFont(font, 12); 

text("Position X: " + dRobot.posVec.x + "\nPosition Y: 
+ "\nPosition Z: " + dRobot.posVec.z, 10, 20); 

text("Servo1: " + serialMsg[o] + "\nServo2: " + serialMsg[1] 
+ "\nServo3: " + serialMsg[2] + "\nGripRot: " + serialMsg[3] 
+ "\nGripWidth: " + serialMsg[4], 10, 80); 


+ dRobot.posVec.y 


And that’s it! The code you just wrote is ready to communicate with Arduino through serial. Now 
you need to write the Arduino code that will translate the information coming from Processing into 
physical movements of the physical robot. 


Arduino Code 


The code running inside your Arduino board has a simple role: it receives serial data that it remaps to the 
range of the servos and sends the appropriate message to each one of them. 

You need variables to store the temporary values of the incoming data and then integers for the 
pin numbers and different pulses for the five servos. The longs previousMillis and interval deal with 
the spacing between messages sent to the servos. 


unsigned int tempHandRot, tempGrip; 
unsigned int servoiPos, servo2Pos, servo3Pos; 


int ledPin = 3; 
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int servo1Pin = 9; 

int pulse1 = 1500; 

int servo2Pin = 10; 

int pulse2 = 1500; 

int servo3Pin = 11; 

int pulse3 = 1500; 

int handRotPin = 5; 

int handRotPulse = 1500; 
int gripPin = 6; 

int gripPulse = 1500; 


long previousMillis = 0; 
long interval = 20; 


int speedServo1 = 0; 
int speedServo2 = 0; 
int speedServo3 = 0; 


int handRotSpeed = 20; 
int gripSpeed = 20; 


Set all your pins as outputs and initialize the serial port. 


void setup() { 
pinMode (ledPin, OUTPUT); 
pinMode (servo1Pin, OUTPUT); 
pinMode (servo2Pin, OUTPUT); 
pinMode (servo3Pin, OUTPUT); 
pinMode (handRotPin, OUTPUT); 
pinMode (gripPin, OUTPUT); 


Serial. begin(9600); // Start serial communication at 9600 bps 


In the main loop, write a high pulse to the LEDs because you're having them on at all times, and 
then check the state of your serial buffer. If you have received more than eight values, which is what 
you are expecting, and the first of them is the trigger character ‘Xx’, you start reading the values as bytes. 
After every two values, yourecompose the integer using the C function word(), which returns a word 
data type (16-bit unsigned number, the same as an unsigned integer) from two bytes. 


void loop() { 
digitalWrite(ledPin, HIGH); 
if (Serial.available()>8) { // If data is available to read, 
char led=Serial.read(); 


if (led=='X'){ 
byte MSB1 = Serial.read(); 
byte LSB1 = Serial.read(); 
servo1Pos = word(MSB1, LSB1); 
byte MSB2 = Serial.read(); 
byte LSB2 = Serial.read(); 
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servo2Pos = word(MSB2, LSB2); 
byte MSB3 = Serial.read(); 
byte LSB3 = Serial.read(); 
servo3Pos = word(MSB3, LSB3); 


tempHandRot = Serial.read(); 
tempGrip = Serial.read(); 


And now youremap the pulses from their expected ranges to the servo range 500-2500. 


pulse1 = (int)map(servo1Pos,0,2000,500,2500) ; 
pulse2 = (int)map(servo2Pos,0,2000,500,2500) ; 
pulse3 = (int)map(servo3Pos,0,2000,500,2500) ; 


handRotPulse = (int)map(tempHandRot, 0,200, 2500, 500) ; 
gripPulse = (int)map(tempGrip,0,220,500,2500); 


And finally, if 20 milliseconds have elapsed, you send the pulses to your servo motors to update 
their angles. Remember that different servos have slightly different ranges, so you should test your 
servos and find the correct range that drives them from 0 to 180 degrees before remapping the values! 


unsigned long currentMillis = millis(); 

if(currentMillis - previousMillis > interval) { 
previousMillis = currentMillis; 
updateServo(servoiPin, pulse1); 
updateServo(servo2Pin, pulse2); 
updateServo(servo3Pin, pulse3); 
updateServo(handRotPin, handRotPulse) ; 
updateServo(gripPin, gripPulse) ; 


} 
} 


void updateServo (int pin, int pulse){ 
digitalWrite(pin, HIGH); 
delayMicroseconds(pulse) ; 
digitalWrite(pin, LOW); 


If all the steps have been implemented appropriately, you should have a working delta robot 
following your hand as you move it in space (Figure 13-38). 
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Figure 13-38. The delta robot in action 


Summary 


This has been quite a long project and its development has required you to master 3D transformations, 
inverse kinematics, hand recognition, point cloud analysis, serial communication, anda long list of 
programming techniques associated with the implementation of a physically accurate simulation ofa 
robot. On the physical side, you worked with ball joints, laser cutting, servos, LEDs, anda series of other 
parts required by the making of a precise robotic platform. 

The possibilities that natural interaction introduce in the world of robotics are yet to be explored. 
Not only are you delivered of using interfacing devices such as a mouse or keyboard, but youare also 
on the verge of a richer and more intuitive communication between humans and machines. Tools like 
Arduino and Kinect have allowed us to bridge the gap between our body kinetics and those of a 
machine within a pretty constrained budget and time scale. And this is only a start. You have now the 
tools to start tinkering with more complex applications for your robot, attaching different effectors, 
trying new forms of interaction. Your only limit is your imagination. 
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onPointUpdate() function, 306 
description, 41 
installation 
Linux, 43 
Mac OS X, 42-43 
Windows, 42 
Skeleton tracking, 58-60, 118 
description, 118 
drawSkeleton() function, 119 
Kinect, 28 
Simple-OpenNI events, 119-120 
SoftwareSerial library, 290 
SparkFun Arduino prototype shield, 319-321 
Structured-light 3D scanning technique 
depth map, 32 
IR coding image, 32 
triangulation principles, 31 
Surface reconstruction 
blender, 342 
color transfer and vertex geometry, 341 
delete points, 339 
3D triangular meshes, 338 
mesh generation, 340 
point cloud, 338 
vertex colors transfer, 341 
Switch() statement, 294 


T 


Tangible table interface 
calibrated point cloud, 193 
finger position, 195-198 
point cloud, 187-195 
rotation equations, 188 
setup, 186 
uncalibrated point cloud, 187 
TextDisplay() function, 238 
Third dimensional (3D) modeling interface 
Arduino LilyPad, 282 
components, 280, 281 
connecting the circuit, 284-286 
flex sensors, 283-284 


geometric classes 
Line class, 298 
Point class, 296-297 
Shape class, 298-299 
glove interface, 279 
GloveInterface class, implementation 
calibrating the interface, 295-296 
display functions, 294-295 
setter functions, 293-294 
main program 
addLine() function, 303 
addPoint() function, 303 
addShape() function, 304 
drawClosestPoint() function, 304 
draw function, 301-302 
imports and fields, 299-300 
movePoint() function, 303 
processing callback functions, 305 
setup function, 300-301 
Simple-OpenNI callback functions, 
306-307 
unSelectAllQ) function, 303, 304 
testing the circuit 
Arduino serial sender, 287 
processing glove data receiver, 287-289 
wireless communication 
Arduino code, 290-291 
LilyPad XBee breakout board, 289-290 
SoftwareSerial library, 290 
Third dimensional (3D) processing, 46 
camera control libraries, 50 
cube, 47 
description, 46 
KinectOrbit, 50-51 
matrix transforms, 47-49 
Tracking 
hand, 57-58, 89 
connection to Arduino, 97 
draw loop, 93-97 
libraries and setup, 89-91 
NITE callbacks, 91-93 
Kinect 
hand, 28 
skeleton, 28 
skeleton, 58-60, 118 
description, 118 
drawSkeleton() function, 119 
Simple-OpenNI events, 119-120 
Turning functions, 223 
Turntable scanner 
assembled system, 309 
building 
gears connections, 314-319 
parts, 313 
prototype, 313 


circuit, 319-321 
point cloud 
exportPly function, 337 
ply and obj extension, 335 
surface reconstruction 
blender, 342 
color transfer and vertex 
geometry, 341 
delete points, 339 
3D triangular meshes, 338 
mesh generation, 340 
point cloud, 338 
vertex colors transfer, 341 
theory 
diagram, 311 
Kinect cameras, 310 
Meshlab, 310 
reconstruction, 311 
requirements, 312 
SLAM and RANSAC-based 
reconstruction, 310 
SpinScan, 310 
trigonometry, 311 
see also Arduino code 
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UnSelectAll() function, 303, 304 
UpdateUserData() function, 267-268 
UpdateUserHeight() function, 268-270 
User control sketch, mood lamps 

data storage and retrieval, 160-161 

description, 154 

display functions, 161-163 

draw() function, 155-156 

serial communication, 161 

setup() function, 154 

Simple-OpenNI callbacks, 163-164 


INDEX 


userControl() function 
lamp control, 157-159 
lamp creation, 159-160 
variable declaration, 154 
User interface, 263-265. See also Third 
dimensional (3D) modeling interface 
User recognition, 270 
vector subtraction, 271-273 
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Vertex() function, 298 
Virtual robot model 
description, 198 
draw() function method, 200-201 
input polishing, 201-203 
pointer projection, 200 
work area, 201 
Voltage divider, 283 
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Wireless interface 
Arduino code, 290-291 
LilyPad XBee breakout board, 289-290 
SoftwareSerial library, 290 
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XBee Explorer for wireless communication, 
230-232 
XBee Explorer USB, 289 
XBee wireless module 
description, 145 
explorer connected to Arduino, 146 
final circuit, 148 
pencil holder, 148 
strip board, with serial connections, 147 
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